Skip to content

Commit

Permalink
Removed baseline distance from RawGNSSAttitudeOutput.
Browse files Browse the repository at this point in the history
This is just the norm of the ENU vector contained in the message, so it's
redundant.
  • Loading branch information
adamshapiro0 committed Dec 3, 2024
1 parent 45df800 commit c117cd8
Show file tree
Hide file tree
Showing 3 changed files with 7 additions and 23 deletions.
5 changes: 3 additions & 2 deletions python/fusion_engine_client/analysis/analyzer.py
Original file line number Diff line number Diff line change
Expand Up @@ -2046,6 +2046,7 @@ def plot_gnss_attitude_measurements(self):
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, :]))
raw_baseline_distance_m = np.linalg.norm(raw_heading_data.relative_position_enu_m, axis=0)

# Compute heading uncertainty envelop.
denom = raw_heading_data.relative_position_enu_m[0]**2 + raw_heading_data.relative_position_enu_m[1]**2
Expand All @@ -2058,7 +2059,7 @@ def plot_gnss_attitude_measurements(self):
)

envelope = np.arctan(
(2 * heading_std / raw_heading_data.baseline_distance_m)
(2 * heading_std / raw_baseline_distance_m)
)
envelope *= 180. / np.pi
fig.add_trace(
Expand Down Expand Up @@ -2147,7 +2148,7 @@ def plot_gnss_attitude_measurements(self):
fig.add_trace(
go.Scatter(
x=raw_heading_time,
y=raw_heading_data.baseline_distance_m,
y=raw_baseline_distance_m,
customdata=raw_heading_data.p1_time,
marker={'size': 2, "color": "red"},
hovertemplate='<b>Time</b>: %{x:.3f} sec (%{customdata:.3f} sec)'
Expand Down
15 changes: 4 additions & 11 deletions python/fusion_engine_client/messages/measurements.py
Original file line number Diff line number Diff line change
Expand Up @@ -1297,7 +1297,7 @@ class RawGNSSAttitudeOutput(MessagePayload):
MESSAGE_TYPE = MessageType.RAW_GNSS_ATTITUDE_OUTPUT
MESSAGE_VERSION = 0

_STRUCT = struct.Struct('<B3xL3f3fff')
_STRUCT = struct.Struct('<B3xL3f3f')

def __init__(self):
## Measurement timestamps, if available. See @ref measurement_messages.
Expand Down Expand Up @@ -1370,9 +1370,7 @@ def unpack(self, buffer: bytes, offset: int = 0, message_version: int = MessageP
self.relative_position_enu_m[2],
self.position_std_enu_m[0],
self.position_std_enu_m[1],
self.position_std_enu_m[2],
self.baseline_distance_m,
self.baseline_distance_std_m) = self._STRUCT.unpack_from(buffer, offset)
self.position_std_enu_m[2]) = self._STRUCT.unpack_from(buffer, offset)
offset += self._STRUCT.size

self.solution_type = SolutionType(solution_type_int)
Expand All @@ -1383,8 +1381,7 @@ 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, ' \
f'baseline={self.baseline_distance_m} m]'
result += f', solution_type={self.solution_type}, enu={enu_str} m, heading={heading_deg:.1f} deg]'
return result

def __str__(self):
Expand All @@ -1393,9 +1390,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}
Baseline distance (m): {self.baseline_distance_m:.2f}
Baseline std (m): {self.baseline_distance_std_m:.2f}"""
Heading (deg): {self.get_heading_deg():.2f}"""

@classmethod
def calcsize(cls) -> int:
Expand All @@ -1408,8 +1403,6 @@ 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,
'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
Expand Down
10 changes: 0 additions & 10 deletions src/point_one/fusion_engine/messages/measurements.h
Original file line number Diff line number Diff line change
Expand Up @@ -1265,16 +1265,6 @@ struct P1_ALIGNAS(4) RawGNSSAttitudeOutput : public MessagePayload {
* resolved with respect to the local ENU tangent plane: east, north, up.
*/
float position_std_enu_m[3] = {NAN, NAN, NAN};

/**
* The estimated distance between primary and secondary antennas (in meters).
*/
float baseline_distance_m = NAN;

/**
* The standard deviation of the baseline distance estimate (in meters).
*/
float baseline_distance_std_m = NAN;
};

////////////////////////////////////////////////////////////////////////////////
Expand Down

0 comments on commit c117cd8

Please sign in to comment.