From 4f23a90f221b5cfdf362a7c4f34d5990021fb7a3 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 6 Nov 2024 12:09:39 -0500 Subject: [PATCH 1/2] Added file read support to binary_message_decode.py. --- python/examples/binary_message_decode.py | 26 ++++++++++++++++-------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/python/examples/binary_message_decode.py b/python/examples/binary_message_decode.py index 793ed1fe..e49561f1 100755 --- a/python/examples/binary_message_decode.py +++ b/python/examples/binary_message_decode.py @@ -19,7 +19,7 @@ if __name__ == "__main__": parser = ArgumentParser(description="""\ Decode and print the contents of one or more FusionEngine messages contained -in a binary string. For example: +in a binary string or file. For example: > python3 binary_message_decode.py \\ 2e31 0000 0acf ee8f 0200 ca32 0000 0000 0400 0000 0000 0000 ff0f 0001 @@ -31,12 +31,16 @@ Source: 0 CRC: 0x8feecf0a Payload: Reset Request [mask=0x01000fff] + +or + +> python3 binary_message_decode.py my_file.p1log """) parser.add_argument('-v', '--verbose', action='count', default=0, help="Print verbose/trace debugging messages.") parser.add_argument('contents', nargs='+', - help="Binary FusionEngine message contents, specified as a hex string. All spaces will be " - "ignored.") + help="Binary FusionEngine message contents, specified as a hex string, or the path to a binary " + "file to be read. All spaces in the hex string will be ignored.") options = parser.parse_args() # Configure logging. @@ -54,13 +58,17 @@ logger = logging.getLogger('point_one.fusion_engine') # Concatenate all hex characters and convert to bytes. - byte_str_array = [b if len(b) % 2 == 0 else f'0{b}' for b in options.contents] - contents_str = ''.join(byte_str_array).replace(' ', '') - if len(contents_str) % 2 != 0: - logger.error("Error: Contents must contain an even number of hex characters.") - sys.exit(1) + if len(options.contents) == 1 and os.path.exists(options.contents[0]): + with open(options.contents[0], 'rb') as f: + contents = f.read() + else: + byte_str_array = [b if len(b) % 2 == 0 else f'0{b}' for b in options.contents] + contents_str = ''.join(byte_str_array).replace(' ', '') + if len(contents_str) % 2 != 0: + logger.error("Error: Contents must contain an even number of hex characters.") + sys.exit(1) - contents = bytes.fromhex(contents_str) + contents = bytes.fromhex(contents_str) # Decode the incoming data and print the contents of any complete messages. decoder = FusionEngineDecoder(warn_on_error=FusionEngineDecoder.WarnOnError.ALL, warn_on_unrecognized=True) From 7aad42fda31e64c3d754ebaf18640aa766721416 Mon Sep 17 00:00:00 2001 From: Adam Shapiro Date: Wed, 6 Nov 2024 12:10:05 -0500 Subject: [PATCH 2/2] Added median vs first position support to _plot_pose_displacement(). --- .../fusion_engine_client/analysis/analyzer.py | 32 +++++++++++++------ 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/python/fusion_engine_client/analysis/analyzer.py b/python/fusion_engine_client/analysis/analyzer.py index 0d084c75..6ea55461 100755 --- a/python/fusion_engine_client/analysis/analyzer.py +++ b/python/fusion_engine_client/analysis/analyzer.py @@ -867,16 +867,16 @@ def plot_stationary_position_error(self, truth_lla_deg): @param truth_lla_deg The truth LLA location (in degrees/meters). """ truth_ecef_m = np.array(geodetic2ecef(*truth_lla_deg, deg=True)) - return self._plot_pose_displacement(title='Position Error', center_ecef_m=truth_ecef_m) + return self._plot_pose_displacement(title='Position Error', reference=truth_ecef_m) def plot_pose_displacement(self): """! - @brief Generate a topocentric (top-down) plot of position displacement, as well as plot of displacement over - time. + @brief Generate a topocentric (top-down) plot of position displacement vs the median position, as well as plot + of displacement over time. """ - return self._plot_pose_displacement() + return self._plot_pose_displacement(reference='median') - def _plot_pose_displacement(self, title='Pose Displacement', center_ecef_m=None): + def _plot_pose_displacement(self, title='Pose Displacement', reference=None): if self.output_dir is None: return None @@ -903,16 +903,30 @@ def _plot_pose_displacement(self, title='Pose Displacement', center_ecef_m=None) # case there are one or two huge outliers). position_ecef_m = np.array(geodetic2ecef(lat=lla_deg[0, :], lon=lla_deg[1, :], alt=lla_deg[2, :], deg=True)) - if center_ecef_m is None: - center_ecef_m = np.median(position_ecef_m, axis=1) + if reference is None: + reference = 'median' + + if isinstance(reference, str): + if reference == 'first': + reference_ecef_m = position_ecef_m[:, 0] + title_suffix = ' From First Position' + elif reference == 'median': + reference_ecef_m = position_ecef_m[:, 0] + title_suffix = ' From Median Position' + else: + raise ValueError('Unrecognized reference specifier.') + else: + reference_ecef_m = reference + title_suffix = '' - displacement_ecef_m = position_ecef_m - center_ecef_m.reshape(3, 1) + displacement_ecef_m = position_ecef_m - reference_ecef_m.reshape(3, 1) c_enu_ecef = get_enu_rotation_matrix(*lla_deg[0:2, 0], deg=True) displacement_enu_m = c_enu_ecef.dot(displacement_ecef_m) axis_title = 'Error' if title == 'Position Error' else 'Displacement' - self._plot_displacement(source=title, title=axis_title, time=time, solution_type=solution_type, + self._plot_displacement(source=f'{title}{title_suffix}', title=axis_title, + time=time, solution_type=solution_type, displacement_enu_m=displacement_enu_m, std_enu_m=std_enu_m) return displacement_enu_m