Removed deprecated HeadingBias config in favor of GNSS aux lever arm …

…specification. (#346)

# New Features
- Added new `GNSS_AUX_LEVER_ARM` (secondary antenna) configuration setting

# Changes
- Removed deprecated `HEADING_BIAS` configuration setting

# Fixes
- Corrected Python `RawGNSSAttitudeOutput` yaw/pitch printout

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.
@@ -745,26 +733,38 @@ 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
 
 
 @_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 primary GNSS antenna with respect to the vehicle body frame, resolved in the vehicle body
+           frame (in meters).
     """
     pass
 
 # Alias for convenience.
 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):
     """!
-    @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
 
@@ -927,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):
     """!

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

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,14 +1406,15 @@ 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}
   UTC time: {utc_str}
   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:

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,16 +53,16 @@ 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 primary GNSS antenna with respect to the vehicle body
+   * frame, resolved in the vehicle body frame (in meters).
    *
    * Payload format: @ref Point3f
    */
   GNSS_LEVER_ARM = 18,
 
   /**
    * 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
    */
@@ -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).
+   *
+   * @deprecated
+   * Use @ref ConfigType::GNSS_AUX_LEVER_ARM instead.
+   */
+  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 HeadingBias
+   * Payload format: @ref Point3f
    */
-  HEADING_BIAS = 23,
+  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