diff --git a/Tools/autotest/unittest/extract_param_defaults_unittest.py b/Tools/autotest/unittest/extract_param_defaults_unittest.py index fe539655470f2..e0921c102d667 100755 --- a/Tools/autotest/unittest/extract_param_defaults_unittest.py +++ b/Tools/autotest/unittest/extract_param_defaults_unittest.py @@ -5,17 +5,28 @@ AP_FLAKE8_CLEAN -Amilcar do Carmo Lucas, IAV GmbH +SPDX-FileCopyrightText: 2024 Amilcar do Carmo Lucas + +SPDX-License-Identifier: GPL-3.0-or-later ''' + import unittest -from unittest.mock import patch, MagicMock -from extract_param_defaults import extract_parameter_default_values, missionplanner_sort, \ - mavproxy_sort, sort_params, output_params, parse_arguments, \ - NO_DEFAULT_VALUES_MESSAGE, MAVLINK_SYSID_MAX, MAVLINK_COMPID_MAX +from unittest.mock import patch +from unittest.mock import MagicMock +from extract_param_defaults import extract_parameter_values +from extract_param_defaults import missionplanner_sort +from extract_param_defaults import mavproxy_sort +from extract_param_defaults import sort_params +from extract_param_defaults import output_params +from extract_param_defaults import parse_arguments +from extract_param_defaults import NO_DEFAULT_VALUES_MESSAGE +from extract_param_defaults import MAVLINK_SYSID_MAX +from extract_param_defaults import MAVLINK_COMPID_MAX -class TestArgParseParameters(unittest.TestCase): + +class TestArgParseParameters(unittest.TestCase): # pylint: disable=missing-class-docstring def test_command_line_arguments_combinations(self): # Check the 'format' and 'sort' default parameters args = parse_arguments(['dummy.bin']) @@ -62,7 +73,7 @@ def test_command_line_arguments_combinations(self): self.assertEqual(args.compid, 3) -class TestExtractParameterDefaultValues(unittest.TestCase): +class TestExtractParameterDefaultValues(unittest.TestCase): # pylint: disable=missing-class-docstring @patch('extract_param_defaults.mavutil.mavlink_connection') def test_logfile_does_not_exist(self, mock_mavlink_connection): @@ -71,7 +82,7 @@ def test_logfile_does_not_exist(self, mock_mavlink_connection): # Call the function with a dummy logfile path with self.assertRaises(SystemExit) as cm: - extract_parameter_default_values('dummy.bin') + extract_parameter_values('dummy.bin') # Check the error message self.assertEqual(str(cm.exception), "Error opening the dummy.bin logfile: Test exception") @@ -88,7 +99,7 @@ def test_extract_parameter_default_values(self, mock_mavlink_connection): ] # Call the function with a dummy logfile path - defaults = extract_parameter_default_values('dummy.bin') + defaults = extract_parameter_values('dummy.bin') # Check if the defaults dictionary contains the correct parameters and values self.assertEqual(defaults, {'PARAM1': 1.1, 'PARAM2': 2.0}) @@ -102,7 +113,7 @@ def test_no_parameters(self, mock_mavlink_connection): # Call the function with a dummy logfile path and assert SystemExit is raised with the correct message with self.assertRaises(SystemExit) as cm: - extract_parameter_default_values('dummy.bin') + extract_parameter_values('dummy.bin') self.assertEqual(str(cm.exception), NO_DEFAULT_VALUES_MESSAGE) @patch('extract_param_defaults.mavutil.mavlink_connection') @@ -114,7 +125,7 @@ def test_no_parameter_defaults(self, mock_mavlink_connection): # Call the function with a dummy logfile path and assert SystemExit is raised with the correct message with self.assertRaises(SystemExit) as cm: - extract_parameter_default_values('dummy.bin') + extract_parameter_values('dummy.bin') self.assertEqual(str(cm.exception), NO_DEFAULT_VALUES_MESSAGE) @patch('extract_param_defaults.mavutil.mavlink_connection') @@ -126,7 +137,7 @@ def test_invalid_parameter_name(self, mock_mavlink_connection): # Call the function with a dummy logfile path with self.assertRaises(SystemExit): - extract_parameter_default_values('dummy.bin') + extract_parameter_values('dummy.bin') @patch('extract_param_defaults.mavutil.mavlink_connection') def test_long_parameter_name(self, mock_mavlink_connection): @@ -137,10 +148,10 @@ def test_long_parameter_name(self, mock_mavlink_connection): # Call the function with a dummy logfile path with self.assertRaises(SystemExit): - extract_parameter_default_values('dummy.bin') + extract_parameter_values('dummy.bin') -class TestSortFunctions(unittest.TestCase): +class TestSortFunctions(unittest.TestCase): # pylint: disable=missing-class-docstring def test_missionplanner_sort(self): # Define a list of parameter names params = ['PARAM_GROUP1_PARAM1', 'PARAM_GROUP2_PARAM2', 'PARAM_GROUP1_PARAM2'] @@ -172,9 +183,9 @@ def test_mavproxy_sort(self): self.assertEqual(sorted_params, ['PARAM1', 'PARAM2', 'PARAM3']) -class TestOutputParams(unittest.TestCase): +class TestOutputParams(unittest.TestCase): # pylint: disable=missing-class-docstring - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params(self, mock_print): # Prepare a dummy defaults dictionary defaults = {'PARAM2': 1.0, 'PARAM1': 2.0} @@ -184,9 +195,11 @@ def test_output_params(self, mock_print): # Check if the print function was called with the correct parameters expected_calls = [unittest.mock.call('PARAM2,1'), unittest.mock.call('PARAM1,2')] + print(mock_print.mock_calls) + print(expected_calls) mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params_missionplanner_non_numeric(self, mock_print): # Prepare a dummy defaults dictionary defaults = {'PARAM1': 'non-numeric'} @@ -198,7 +211,7 @@ def test_output_params_missionplanner_non_numeric(self, mock_print): expected_calls = [unittest.mock.call('PARAM1,non-numeric')] mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params_mavproxy(self, mock_print): # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} @@ -208,11 +221,11 @@ def test_output_params_mavproxy(self, mock_print): output_params(defaults, 'mavproxy') # Check if the print function was called with the correct parameters - expected_calls = [unittest.mock.call("%-15s %.6f" % ('PARAM1', 1.0)), - unittest.mock.call("%-15s %.6f" % ('PARAM2', 2.0))] + expected_calls = [unittest.mock.call("%-15s %.6f" % ('PARAM1', 1.0)), # pylint: disable=consider-using-f-string + unittest.mock.call("%-15s %.6f" % ('PARAM2', 2.0))] # pylint: disable=consider-using-f-string mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params_qgcs(self, mock_print): # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} @@ -223,11 +236,11 @@ def test_output_params_qgcs(self, mock_print): # Check if the print function was called with the correct parameters expected_calls = [unittest.mock.call("\n# # Vehicle-Id Component-Id Name Value Type\n"), - unittest.mock.call("%u %u %-15s %.6f %u" % (1, 1, 'PARAM1', 1.0, 9)), - unittest.mock.call("%u %u %-15s %.6f %u" % (1, 1, 'PARAM2', 2.0, 9))] + unittest.mock.call("%u %u %-15s %.6f %u" % (1, 1, 'PARAM1', 1.0, 9)), # pylint: disable=consider-using-f-string + unittest.mock.call("%u %u %-15s %.6f %u" % (1, 1, 'PARAM2', 2.0, 9))] # pylint: disable=consider-using-f-string mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params_qgcs_2_4(self, mock_print): # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} @@ -238,12 +251,12 @@ def test_output_params_qgcs_2_4(self, mock_print): # Check if the print function was called with the correct parameters expected_calls = [unittest.mock.call("\n# # Vehicle-Id Component-Id Name Value Type\n"), - unittest.mock.call("%u %u %-15s %.6f %u" % (2, 4, 'PARAM1', 1.0, 9)), - unittest.mock.call("%u %u %-15s %.6f %u" % (2, 4, 'PARAM2', 2.0, 9))] + unittest.mock.call("%u %u %-15s %.6f %u" % (2, 4, 'PARAM1', 1.0, 9)), # pylint: disable=consider-using-f-string + unittest.mock.call("%u %u %-15s %.6f %u" % (2, 4, 'PARAM2', 2.0, 9))] # pylint: disable=consider-using-f-string mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') - def test_output_params_qgcs_SYSID_THISMAV(self, mock_print): + @patch('builtins.print') + def test_output_params_qgcs_SYSID_THISMAV(self, mock_print): # pylint: disable=invalid-name # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0, 'SYSID_THISMAV': 3.0} @@ -253,13 +266,13 @@ def test_output_params_qgcs_SYSID_THISMAV(self, mock_print): # Check if the print function was called with the correct parameters expected_calls = [unittest.mock.call("\n# # Vehicle-Id Component-Id Name Value Type\n"), - unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'PARAM1', 1.0, 9)), - unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'PARAM2', 2.0, 9)), - unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'SYSID_THISMAV', 3.0, 9))] + unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'PARAM1', 1.0, 9)), # pylint: disable=consider-using-f-string + unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'PARAM2', 2.0, 9)), # pylint: disable=consider-using-f-string + unittest.mock.call("%u %u %-15s %.6f %u" % (3, 7, 'SYSID_THISMAV', 3.0, 9))] # pylint: disable=consider-using-f-string mock_print.assert_has_calls(expected_calls, any_order=False) - @patch('extract_param_defaults.print') - def test_output_params_qgcs_SYSID_INVALID(self, mock_print): + @patch('builtins.print') + def test_output_params_qgcs_SYSID_INVALID(self, _mock_print): # pylint: disable=invalid-name # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0, 'SYSID_THISMAV': -1.0} @@ -275,8 +288,8 @@ def test_output_params_qgcs_SYSID_INVALID(self, mock_print): output_params(defaults, 'qgcs', MAVLINK_SYSID_MAX+2, 7) self.assertEqual(str(cm.exception), f"Invalid system ID parameter 16777218 must be smaller than {MAVLINK_SYSID_MAX}") - @patch('extract_param_defaults.print') - def test_output_params_qgcs_COMPID_INVALID(self, mock_print): + @patch('builtins.print') + def test_output_params_qgcs_COMPID_INVALID(self, _mock_print): # pylint: disable=invalid-name # Prepare a dummy defaults dictionary defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} @@ -292,7 +305,7 @@ def test_output_params_qgcs_COMPID_INVALID(self, mock_print): output_params(defaults, 'qgcs', 1, MAVLINK_COMPID_MAX+3) self.assertEqual(str(cm.exception), f"Invalid component ID parameter 259 must be smaller than {MAVLINK_COMPID_MAX}") - @patch('extract_param_defaults.print') + @patch('builtins.print') def test_output_params_integer(self, mock_print): # Prepare a dummy defaults dictionary with an integer value defaults = {'PARAM1': 1.01, 'PARAM2': 2.00} @@ -306,5 +319,6 @@ def test_output_params_integer(self, mock_print): mock_print.assert_has_calls(expected_calls, any_order=False) + if __name__ == '__main__': unittest.main() diff --git a/Tools/scripts/extract_param_defaults.py b/Tools/scripts/extract_param_defaults.py index cf50e16ab21e5..e9d01f0455520 100755 --- a/Tools/scripts/extract_param_defaults.py +++ b/Tools/scripts/extract_param_defaults.py @@ -1,7 +1,7 @@ #!/usr/bin/python3 ''' -Extracts parameter default values from an ArduPilot .bin log file. +Extracts parameter values or parameter default values from an ArduPilot .bin log file. Supports Mission Planner, MAVProxy and QGCS file format output @@ -9,7 +9,9 @@ AP_FLAKE8_CLEAN -Amilcar do Carmo Lucas, IAV GmbH +SPDX-FileCopyrightText: 2024 Amilcar do Carmo Lucas + +SPDX-License-Identifier: GPL-3.0-or-later ''' import argparse @@ -22,6 +24,7 @@ PARAM_NAME_MAX_LEN = 16 MAVLINK_SYSID_MAX = 2**24 MAVLINK_COMPID_MAX = 2**8 +MAV_PARAM_TYPE_REAL32 = 9 def parse_arguments(args=None): @@ -43,7 +46,7 @@ def parse_arguments(args=None): choices=['none', 'missionplanner', 'mavproxy', 'qgcs'], default='', help='Sort the parameters in the file. Defaults to the same as the format.', ) - parser.add_argument('-v', '--version', action='version', version='%(prog)s 1.0', + parser.add_argument('-v', '--version', action='version', version='%(prog)s 1.1', help='Display version information and exit.', ) parser.add_argument('-i', '--sysid', type=int, default=-1, @@ -52,6 +55,10 @@ def parse_arguments(args=None): parser.add_argument('-c', '--compid', type=int, default=-1, help='Component ID for qgcs output format. Defaults to 1.', ) + parser.add_argument('-t', '--type', + choices=['defaults', 'values', 'non_default_values'], + default='defaults', help='Type of parameter values to extract. Defaults to %(default)s.', + ) parser.add_argument('bin_file', help='The ArduPilot .bin log file to read') args, _ = parser.parse_known_args(args) @@ -67,35 +74,47 @@ def parse_arguments(args=None): return args -def extract_parameter_default_values(logfile: str) -> Dict[str, float]: +def extract_parameter_values(logfile: str, param_type: str = 'defaults') -> Dict[str, float]: # pylint: disable=too-many-branches """ - Extracts the parameter default values from an ArduPilot .bin log file. + Extracts the parameter values from an ArduPilot .bin log file. Args: logfile: The path to the ArduPilot .bin log file. + param_type: The type of parameter values to extract. Can be 'defaults', 'values' or 'non_default_values'. Returns: - A dictionary with parameter names as keys and their default values as float. + A dictionary with parameter names as keys and their values as float. """ try: mlog = mavutil.mavlink_connection(logfile) except Exception as e: - raise SystemExit("Error opening the %s logfile: %s" % (logfile, str(e))) from e - defaults = {} + raise SystemExit(f"Error opening the {logfile} logfile: {str(e)}") from e + values = {} while True: m = mlog.recv_match(type=['PARM']) if m is None: - if not defaults: + if not values: raise SystemExit(NO_DEFAULT_VALUES_MESSAGE) - return defaults + return values pname = m.Name if len(pname) > PARAM_NAME_MAX_LEN: - raise SystemExit("Too long parameter name: %s" % pname) + raise SystemExit(f"Too long parameter name: {pname}") if not re.match(PARAM_NAME_REGEX, pname): - raise SystemExit("Invalid parameter name %s" % pname) + raise SystemExit(f"Invalid parameter name {pname}") # parameter names are supposed to be unique - if pname not in defaults and hasattr(m, 'Default'): - defaults[pname] = m.Default # the first time default is declared is enough + if pname in values: + continue + if param_type == 'defaults': + if hasattr(m, 'Default'): + values[pname] = m.Default + elif param_type == 'values': + if hasattr(m, 'Value'): + values[pname] = m.Value + elif param_type == 'non_default_values': + if hasattr(m, 'Value') and hasattr(m, 'Default') and m.Value != m.Default: + values[pname] = m.Value + else: + raise SystemExit(f"Invalid type {param_type}") def missionplanner_sort(item: str) -> Tuple[str, ...]: @@ -126,33 +145,33 @@ def mavproxy_sort(item: str) -> str: return item -def sort_params(defaults: Dict[str, float], sort_type: str = 'none') -> Dict[str, float]: +def sort_params(params: Dict[str, float], sort_type: str = 'none') -> Dict[str, float]: """ Sorts parameter names according to sort_type Args: - defaults: A dictionary with parameter names as keys and their default values as float. + params: A dictionary with parameter names as keys and their values as float. sort_type: The type of sorting to apply. Can be 'none', 'missionplanner', 'mavproxy' or 'qgcs'. Returns: - A dictionary with parameter names as keys and their default values as float. + A dictionary with parameter names as keys and their values as float. """ if sort_type == "missionplanner": - defaults = dict(sorted(defaults.items(), key=lambda x: missionplanner_sort(x[0]))) + params = dict(sorted(params.items(), key=lambda x: missionplanner_sort(x[0]))) elif sort_type == "mavproxy": - defaults = dict(sorted(defaults.items(), key=lambda x: mavproxy_sort(x[0]))) + params = dict(sorted(params.items(), key=lambda x: mavproxy_sort(x[0]))) elif sort_type == "qgcs": - defaults = {k: defaults[k] for k in sorted(defaults)} - return defaults + params = {k: params[k] for k in sorted(params)} + return params -def output_params(defaults: Dict[str, float], format_type: str = 'missionplanner', +def output_params(params: Dict[str, float], format_type: str = 'missionplanner', # pylint: disable=too-many-branches sysid: int = -1, compid: int = -1) -> None: """ - Outputs parameters names and their default values to the console + Outputs parameters names and their values to the console Args: - defaults: A dictionary with parameter names as keys and their default values as float. + params: A dictionary with parameter names as keys and their values as float. format_type: The output file format. Can be 'missionplanner', 'mavproxy' or 'qgcs'. Returns: @@ -160,45 +179,43 @@ def output_params(defaults: Dict[str, float], format_type: str = 'missionplanner """ if format_type == "qgcs": if sysid == -1: - if 'SYSID_THISMAV' in defaults: - sysid = defaults['SYSID_THISMAV'] + if 'SYSID_THISMAV' in params: + sysid = int(params['SYSID_THISMAV']) else: sysid = 1 # if unspecified, default to 1 if compid == -1: compid = 1 # if unspecified, default to 1 if sysid < 0: - raise SystemExit("Invalid system ID parameter %i must not be negative" % sysid) + raise SystemExit(f"Invalid system ID parameter {sysid} must not be negative") if sysid > MAVLINK_SYSID_MAX-1: - raise SystemExit("Invalid system ID parameter %i must be smaller than %i" % (sysid, MAVLINK_SYSID_MAX)) + raise SystemExit(f"Invalid system ID parameter {sysid} must be smaller than {MAVLINK_SYSID_MAX}") if compid < 0: - raise SystemExit("Invalid component ID parameter %i must not be negative" % compid) + raise SystemExit(f"Invalid component ID parameter {compid} must not be negative") if compid > MAVLINK_COMPID_MAX-1: - raise SystemExit("Invalid component ID parameter %i must be smaller than %i" % (compid, MAVLINK_COMPID_MAX)) + raise SystemExit(f"Invalid component ID parameter {compid} must be smaller than {MAVLINK_COMPID_MAX}") # see https://dev.qgroundcontrol.com/master/en/file_formats/parameters.html print(""" # # Vehicle-Id Component-Id Name Value Type """) - for param_name, default_value in defaults.items(): + for param_name, param_value in params.items(): if format_type == "missionplanner": try: - default_value = format(default_value, '.6f').rstrip('0').rstrip('.') + param_value = format(param_value, '.6f').rstrip('0').rstrip('.') except ValueError: pass # preserve non-floating point strings, if present - print(f"{param_name},{default_value}") + print(f"{param_name},{param_value}") elif format_type == "mavproxy": - print("%-15s %.6f" % (param_name, default_value)) + print(f"{param_name:<15} {param_value:.6f}") elif format_type == "qgcs": - MAV_PARAM_TYPE_REAL32 = 9 - print("%u %u %-15s %.6f %u" % - (sysid, compid, param_name, default_value, MAV_PARAM_TYPE_REAL32)) + print(f"{sysid} {compid} {param_name:<15} {param_value:.6f} {MAV_PARAM_TYPE_REAL32}") def main(): args = parse_arguments() - parameter_default_values = extract_parameter_default_values(args.bin_file) - parameter_default_values = sort_params(parameter_default_values, args.sort) - output_params(parameter_default_values, args.format, args.sysid, args.compid) + parameter_values = extract_parameter_values(args.bin_file, args.type) + parameter_values = sort_params(parameter_values, args.sort) + output_params(parameter_values, args.format, args.sysid, args.compid) if __name__ == "__main__":