From daa4b61d554ee45834c9136d51d6256f89e97ee7 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 4 Dec 2024 11:33:13 -0500 Subject: [PATCH 1/4] Clarified lever arm resolving frame documentation. --- python/fusion_engine_client/messages/configuration.py | 9 ++++++--- src/point_one/fusion_engine/messages/configuration.h | 10 +++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/python/fusion_engine_client/messages/configuration.py b/python/fusion_engine_client/messages/configuration.py index e8ff9b2b..d6b9cf5c 100644 --- a/python/fusion_engine_client/messages/configuration.py +++ b/python/fusion_engine_client/messages/configuration.py @@ -745,7 +745,8 @@ class Empty(NamedTuple): @_conf_gen.create_config_class(ConfigType.DEVICE_LEVER_ARM, _conf_gen.Point3FConstruct) class DeviceLeverArmConfig(_conf_gen.Point3F): """! - @brief The location of the device IMU with respect to the vehicle body frame (in meters). + @brief The location of the device IMU with respect to the vehicle body frame, resolved in the vehicle body frame (in + meters). """ pass @@ -753,7 +754,8 @@ class DeviceLeverArmConfig(_conf_gen.Point3F): @_conf_gen.create_config_class(ConfigType.GNSS_LEVER_ARM, _conf_gen.Point3FConstruct) class GNSSLeverArmConfig(_conf_gen.Point3F): """! - @brief The location of the GNSS antenna with respect to the vehicle body frame (in meters). + @brief The location of the GNSS antenna with respect to the vehicle body frame, resolved in the vehicle body frame + (in meters). """ pass @@ -764,7 +766,8 @@ class GNSSLeverArmConfig(_conf_gen.Point3F): @_conf_gen.create_config_class(ConfigType.OUTPUT_LEVER_ARM, _conf_gen.Point3FConstruct) class OutputLeverArmConfig(_conf_gen.Point3F): """! - @brief The location of the desired output location with respect to the vehicle body frame (in meters). + @brief The location of the desired output location with respect to the vehicle body frame, resolved in the vehicle + body frame (in meters). """ pass diff --git a/src/point_one/fusion_engine/messages/configuration.h b/src/point_one/fusion_engine/messages/configuration.h index 042160d6..c074bd2d 100644 --- a/src/point_one/fusion_engine/messages/configuration.h +++ b/src/point_one/fusion_engine/messages/configuration.h @@ -38,8 +38,8 @@ enum class ConfigType : uint16_t { INVALID = 0, /** - * The location of the device IMU with respect to the vehicle body frame (in - * meters). + * The location of the device IMU with respect to the vehicle body frame, + * resolved in the vehicle body frame (in meters). * * Payload format: @ref Point3f */ @@ -53,8 +53,8 @@ enum class ConfigType : uint16_t { DEVICE_COARSE_ORIENTATION = 17, /** - * The location of the GNSS antenna with respect to the vehicle body frame (in - * meters). + * The location of the GNSS antenna with respect to the vehicle body frame, + * resolved in the vehicle body frame (in meters). * * Payload format: @ref Point3f */ @@ -62,7 +62,7 @@ enum class ConfigType : uint16_t { /** * The offset of the desired output location with respect to the vehicle - * body frame (in meters). + * body frame, resolved in the vehicle body frame (in meters). * * Payload format: @ref Point3f */ From 3d19ea5e844ae120f1b84f1aedea468798e1c395 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Tue, 3 Dec 2024 16:52:47 -0500 Subject: [PATCH 2/4] Removed deprecated HeadingBias config in favor of GNSS aux lever arm. --- .../messages/configuration.py | 37 ++++------ .../fusion_engine/messages/configuration.h | 70 ++++++------------- 2 files changed, 35 insertions(+), 72 deletions(-) diff --git a/python/fusion_engine_client/messages/configuration.py b/python/fusion_engine_client/messages/configuration.py index d6b9cf5c..b74500d7 100644 --- a/python/fusion_engine_client/messages/configuration.py +++ b/python/fusion_engine_client/messages/configuration.py @@ -30,7 +30,8 @@ class ConfigType(IntEnum): VEHICLE_DETAILS = 20 WHEEL_CONFIG = 21 HARDWARE_TICK_CONFIG = 22 - HEADING_BIAS = 23 + DEPRECATED_HEADING_BIAS = 23 + GNSS_AUX_LEVER_ARM = 24 ENABLED_GNSS_SYSTEMS = 50 ENABLED_GNSS_FREQUENCY_BANDS = 51 LEAP_SECOND = 52 @@ -654,19 +655,6 @@ class HardwareTickConfig(NamedTuple): "wheel_ticks_to_m" / Float32l, ) - class HeadingBias(NamedTuple): - """! - @brief Horizontal and vertical heading bias configuration settings. - """ - horizontal_bias_deg: float = math.nan - vertical_bias_deg: float = math.nan - - - HeadingBiasConstruct = Struct( - "horizontal_bias_deg" / Float32l, - "vertical_bias_deg" / Float32l, - ) - class IonosphereConfig(NamedTuple): """! @brief Ionospheric delay model configuration. @@ -754,8 +742,8 @@ class DeviceLeverArmConfig(_conf_gen.Point3F): @_conf_gen.create_config_class(ConfigType.GNSS_LEVER_ARM, _conf_gen.Point3FConstruct) class GNSSLeverArmConfig(_conf_gen.Point3F): """! - @brief The location of the GNSS antenna with respect to the vehicle body frame, resolved in the vehicle body frame - (in meters). + @brief The location of the primary GNSS antenna with respect to the vehicle body frame, resolved in the vehicle body + frame (in meters). """ pass @@ -763,6 +751,15 @@ class GNSSLeverArmConfig(_conf_gen.Point3F): GnssLeverArmConfig = GNSSLeverArmConfig +@_conf_gen.create_config_class(ConfigType.GNSS_AUX_LEVER_ARM, _conf_gen.Point3FConstruct) +class GNSSAuxLeverArmConfig(_conf_gen.Point3F): + """! + @brief The location of the secondary GNSS antenna with respect to the vehicle body frame on a dual-antenna platform, + resolved in the vehicle body frame (in meters). + """ + pass + + @_conf_gen.create_config_class(ConfigType.OUTPUT_LEVER_ARM, _conf_gen.Point3FConstruct) class OutputLeverArmConfig(_conf_gen.Point3F): """! @@ -930,14 +927,6 @@ class HardwareTickConfig(_conf_gen.HardwareTickConfig): pass -@_conf_gen.create_config_class(ConfigType.HEADING_BIAS, _conf_gen.HeadingBiasConstruct) -class HeadingBias(_conf_gen.HeadingBias): - """! - @brief Horizontal and vertical heading bias. - """ - pass - - @_conf_gen.create_interface_config_class(InterfaceConfigType.BAUD_RATE, _conf_gen.UInt32Construct) class InterfaceBaudRateConfig(_conf_gen.IntegerVal): """! diff --git a/src/point_one/fusion_engine/messages/configuration.h b/src/point_one/fusion_engine/messages/configuration.h index c074bd2d..f8668136 100644 --- a/src/point_one/fusion_engine/messages/configuration.h +++ b/src/point_one/fusion_engine/messages/configuration.h @@ -53,8 +53,8 @@ enum class ConfigType : uint16_t { DEVICE_COARSE_ORIENTATION = 17, /** - * The location of the GNSS antenna with respect to the vehicle body frame, - * resolved in the vehicle body frame (in meters). + * The location of the primary GNSS antenna with respect to the vehicle body + * frame, resolved in the vehicle body frame (in meters). * * Payload format: @ref Point3f */ @@ -101,11 +101,24 @@ enum class ConfigType : uint16_t { /** * Used to set horizontal (yaw) & vertical (pitch) biases (in degrees) on - * a dual-antenna heading platform configuration. + * a dual-antenna heading platform configuration (deprecated). * - * Payload format: @ref HeadingBias + * @deprecated + * Use @ref ConfigType::GNSS_AUX_LEVER_ARM instead. */ - HEADING_BIAS = 23, + DEPRECATED_HEADING_BIAS = 23, + + /** + * The location of the secondary GNSS antenna with respect to the vehicle body + * frame on a dual-antenna platform, resolved in the vehicle body frame (in + * meters). + * + * For dual-antenna systems, the secondary or auxiliary antenna is used to + * measure vehicle yaw and pitch. + * + * Payload format: @ref Point3f + */ + GNSS_AUX_LEVER_ARM = 24, /** * A bitmask indicating which GNSS constellations are enabled. @@ -287,9 +300,12 @@ P1_CONSTEXPR_FUNC const char* to_string(ConfigType type) { case ConfigType::HARDWARE_TICK_CONFIG: return "Hardware Tick Config"; - case ConfigType::HEADING_BIAS: + case ConfigType::DEPRECATED_HEADING_BIAS: return "Heading Bias"; + case ConfigType::GNSS_AUX_LEVER_ARM: + return "GNSS Aux Lever Arm"; + case ConfigType::ENABLED_GNSS_SYSTEMS: return "Enabled GNSS Systems"; @@ -1213,48 +1229,6 @@ struct P1_ALIGNAS(4) HardwareTickConfig { float wheel_ticks_to_m = NAN; }; -/** - * @brief Heading bias horizontal/vertical configuration settings. - * @ingroup config_types - * - * @note - * Both HeadingBias values must be set for the system to use them. - * If one value is NOT set, the system will not output the corrected - * heading message. - * - * @ref GNSSAttitudeOutput - */ -struct P1_ALIGNAS(4) HeadingBias { - /** - * The offset between the heading measured by a secondary GNSS device and the - * vehicle's direction of motion in the horizontal plane (defined by the - * vehicle's forward and left axes). - * - * Bias is defined as the angle between the vector pointing from the primary - * GNSS antenna to the secondary heading antenna, and the vector pointing from - * the primary antenna pointing in the forward direction of the vehicle. A - * positive angle means the secondary antenna is offset in a counter-clockwise - * direction from the forward vector (positive yaw rotation). - * - * For example, if the primary antenna is in the back of the vehicle and the - * secondary antenna is in the front, a positive angle would indicate that the - * secondary antenna is offset to the left side of the vehicle. - */ - float horizontal_bias_deg = NAN; - - /** - * The offset between the heading measured by a secondary GNSS device and the - * vehicle's direction of motion in the vertical plane (defined by the - * vehicle's forward and up axes). - * - * A positive angle means the secondary antenna is offset in the downward - * direction. For example, if the primary antenna is in the back of the - * vehicle and the secondary antenna is in the front, a positive angle would - * indicate that the secondary antenna is mounted below the primary antenna. - */ - float vertical_bias_deg = NAN; -}; - /** * @brief The ionospheric delay model to use. * @ingroup config_types From 5cbdf79207b91cd2d560c164b031bedaa4ce5651 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 12 Dec 2024 13:55:37 -0500 Subject: [PATCH 3/4] Added angle wrapping in yaw/heading conversion functions. --- python/fusion_engine_client/messages/defs.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/python/fusion_engine_client/messages/defs.py b/python/fusion_engine_client/messages/defs.py index 84fab7dc..1a8db584 100644 --- a/python/fusion_engine_client/messages/defs.py +++ b/python/fusion_engine_client/messages/defs.py @@ -796,9 +796,19 @@ def PackedDataToBuffer(packed_data: bytes, buffer: Optional[bytes] = None, offse return len(packed_data) -def yaw_to_heading(yaw_deg: Union[float, np.ndarray]): - return 90.0 - yaw_deg +def yaw_to_heading(yaw: Union[float, np.ndarray], deg: bool = True): + if deg: + heading_deg = 90.0 - yaw + return np.fmod(heading_deg + 180.0, 360.0) + else: + heading_rad = math.pi / 2.0 - yaw + return np.fmod(heading_rad + math.pi, 2.0 * math.pi) -def heading_to_yaw(heading_deg: Union[float, np.ndarray]): - return 90.0 - heading_deg +def heading_to_yaw(heading: Union[float, np.ndarray], deg: bool = True): + if deg: + yaw_deg = 90.0 - heading + return np.fmod(yaw_deg + 180.0, 360.0) - 180.0 + else: + yaw_rad = math.pi / 2.0 - heading + return np.fmod(yaw_rad + math.pi, 2.0 * math.pi) - math.pi From 2cd422dcd84786c9d53da6aaa51e9d0e3bccc28b Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Thu, 12 Dec 2024 19:26:41 -0500 Subject: [PATCH 4/4] Fixed GNSS attitude yaw/pitch printout. --- .../messages/measurements.py | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/python/fusion_engine_client/messages/measurements.py b/python/fusion_engine_client/messages/measurements.py index cc6382d5..ad903802 100644 --- a/python/fusion_engine_client/messages/measurements.py +++ b/python/fusion_engine_client/messages/measurements.py @@ -1258,8 +1258,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP def __repr__(self): result = super().__repr__()[:-1] - ypr_str = '(%.1f, %.1f, %.1f)' % tuple(self.ypr_deg) - result += f', solution_type={self.solution_type}, ypr={ypr_str} deg, ' \ + ypr_str = '(%.1f, %.1f, %.1f) deg' % tuple(self.ypr_deg) + result += f', solution_type={self.solution_type}, ypr={ypr_str}, ' \ f'baseline={self.baseline_distance_m} m]' return result @@ -1335,8 +1335,13 @@ def __init__(self): # 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 get_ypr_deg(self): + ypr_rad = np.array(( + math.atan2(self.relative_position_enu_m[1], self.relative_position_enu_m[0]), + -math.atan2(self.relative_position_enu_m[2], np.linalg.norm(self.relative_position_enu_m[:2])), + np.nan + )) + return np.rad2deg(ypr_rad) def pack(self, buffer: bytes = None, offset: int = 0, return_buffer: bool = True) -> (bytes, int): if buffer is None: @@ -1389,8 +1394,8 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP def __repr__(self): result = super().__repr__()[:-1] 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]' + ypr_str = '(%.1f, %.1f, %.1f) deg' % tuple(self.get_ypr_deg()) + result += f', solution_type={self.solution_type}, enu={enu_str} m, ypr={ypr_str}]' return result def __str__(self): @@ -1401,6 +1406,7 @@ def __str__(self): else: gps_str = 'None' utc_str = 'None' + ypr_deg = self.get_ypr_deg() return f"""\ Raw GNSS Attitude Output @ {str(self.details.p1_time)} GPS time: {gps_str} @@ -1408,7 +1414,7 @@ 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.get_heading_deg():.2f}""" + YPR (deg): {ypr_deg[0]:.2f}, {ypr_deg[1]:.2f}, {ypr_deg[2]:.2f}""" @classmethod def calcsize(cls) -> int: