Skip to content

Removed deprecated HeadingBias config in favor of GNSS aux lever arm specification. #346

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
adamshapiro0 merged 4 commits into from
Dec 18, 2024
Merged

Removed deprecated HeadingBias config in favor of GNSS aux lever arm specification. #346

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
daa4b61
Clarified lever arm resolving frame documentation.
adamshapiro0 Dec 4, 2024
3d19ea5
Removed deprecated HeadingBias config in favor of GNSS aux lever arm.
adamshapiro0 Dec 3, 2024
5cbdf79
Added angle wrapping in yaw/heading conversion functions.
adamshapiro0 Dec 12, 2024
2cd422d
Fixed GNSS attitude yaw/pitch printout.
adamshapiro0 Dec 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 17 additions & 25 deletions python/fusion_engine_client/messages/configuration.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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):
"""!
Expand Down
18 changes: 14 additions & 4 deletions python/fusion_engine_client/messages/defs.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
20 changes: 13 additions & 7 deletions python/fusion_engine_client/messages/measurements.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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):
Expand All @@ -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:
Expand Down
76 changes: 25 additions & 51 deletions src/point_one/fusion_engine/messages/configuration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -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
*/
Expand Down Expand Up @@ -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.
Expand Down Expand Up @@ -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";

Expand Down Expand Up @@ -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
Expand Down
Loading