-
Notifications
You must be signed in to change notification settings - Fork 17.6k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Tools: Extracts parameter default values from an ArduPilot .bin file.
Supports Mission Planner, MAVProxy and QGCS file format output Optionally sorts the parameters Contains unittests with 84% coverage Amilcar do Carmo Lucas, IAV GmbH
- Loading branch information
1 parent
1ec61c5
commit 78623d9
Showing
2 changed files
with
318 additions
and
0 deletions.
There are no files selected for viewing
142 changes: 142 additions & 0 deletions
142
Tools/autotest/param_metadata/extract_param_defaults.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,142 @@ | ||
#!/usr/bin/python3 | ||
|
||
''' | ||
Extracts parameter default values from an ArduPilot .bin file. | ||
Supports Mission Planner, MAVProxy and QGCS file format output | ||
Optionally sorts the parameters | ||
Currently has 84% unit test coverage | ||
AP_FLAKE8_CLEAN | ||
Amilcar do Carmo Lucas, IAV GmbH | ||
''' | ||
|
||
import argparse | ||
import re | ||
from typing import Dict, Tuple | ||
from pymavlink import mavutil | ||
|
||
NO_DEFAULT_VALUES_MESSAGE = "The .bin file contained no parameter default values. Update to a newer ArduPilot firmware version" | ||
|
||
|
||
def extract_parameter_default_values(logfile: str) -> Dict[str, float]: | ||
""" | ||
Extracts the parameter default values from an ArduPilot .bin log file. | ||
Args: | ||
logfile: The path to the ArduPilot .bin log file. | ||
Returns: | ||
A dictionary with parameter names as keys and their default values as float. | ||
""" | ||
mlog = mavutil.mavlink_connection(logfile) | ||
defaults = {} | ||
while True: | ||
m = mlog.recv_match(type=['PARM']) | ||
if m is None: | ||
if not defaults: | ||
print(NO_DEFAULT_VALUES_MESSAGE) | ||
return defaults | ||
pname = m.Name | ||
if len(pname) > 16: | ||
print("Too long parameter name: %s" % pname) | ||
raise SystemExit("Too long parameter name") | ||
if not re.match(r'^[A-Z][A-Z_0-9]*$', pname): | ||
print("Invalid parameter name %s" % pname) | ||
raise SystemExit("Invalid parameter name") | ||
# 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 | ||
|
||
|
||
def missionplanner_sort(item: str) -> Tuple[str, ...]: | ||
""" | ||
Sorts a parameter name according to the rules defined in the Mission Planner software. | ||
Args: | ||
item: The parameter name to sort. | ||
Returns: | ||
A tuple representing the sorted parameter name. | ||
""" | ||
parts = item.split("_") # Split the parameter name by underscore | ||
# Compare the parts separately | ||
return tuple(parts) | ||
|
||
|
||
def mavproxy_sort(item: str) -> str: | ||
""" | ||
Sorts a parameter name according to the rules defined in the MAVProxy software. | ||
Args: | ||
item: The parameter name to sort. | ||
Returns: | ||
The sorted parameter name. | ||
""" | ||
return item | ||
|
||
|
||
def output_params(defaults: Dict[str, float], format_type: str = 'missionplanner', sort_type: str = 'none') -> None: | ||
""" | ||
Outputs the parameters names and their default values to the console, optionally sorted. | ||
Args: | ||
defaults: A dictionary with parameter names as keys and their default values as float. | ||
format_type: The output file format. Can be 'missionplanner', 'mavproxy' or 'qgcs'. | ||
sort_type: The type of sorting to apply. Can be 'none', 'missionplanner', 'mavproxy' or 'qgcs'. | ||
Returns: | ||
None | ||
""" | ||
if sort_type == "missionplanner": | ||
defaults = dict(sorted(defaults.items(), key=lambda x: missionplanner_sort(x[0]))) | ||
elif sort_type == "mavproxy": | ||
defaults = dict(sorted(defaults.items(), key=lambda x: mavproxy_sort(x[0]))) | ||
elif sort_type == "qgcs": | ||
defaults = {k: defaults[k] for k in sorted(defaults)} | ||
|
||
if format_type == "qgcs": | ||
# 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(): | ||
if format_type == "missionplanner": | ||
try: | ||
default_value = format(default_value, '.6f').rstrip('0').rstrip('.') | ||
except ValueError: | ||
pass | ||
print(f"{param_name},{default_value}") | ||
elif format_type == "mavproxy": | ||
print("%-15s %.6f" % (param_name, default_value)) | ||
elif format_type == "qgcs": | ||
MAV_PARAM_TYPE_REAL32 = 9 | ||
sysid = 1 | ||
compid = 1 | ||
print("%u %u %-15s %.6f %u" % | ||
(sysid, compid, param_name, default_value, MAV_PARAM_TYPE_REAL32)) | ||
|
||
|
||
def main(): | ||
parser = argparse.ArgumentParser(description='Extracts parameter default values from an ArduPilot .bin file.') | ||
parser.add_argument('-f', '--format', | ||
choices=['missionplanner', 'mavproxy', 'qgcs'], | ||
default='missionplanner', help='Output file format. Defaults to missionplanner.', | ||
) | ||
parser.add_argument('-s', '--sort', | ||
choices=['none', 'missionplanner', 'mavproxy', 'qgcs'], | ||
default='none', help='Sort the parameters in the file. Defaults to not sorting.', | ||
) | ||
parser.add_argument('bin_file', help='The Ardupilot .bin log file to read') | ||
args = parser.parse_args() | ||
|
||
parameter_default_values = extract_parameter_default_values(args.bin_file) | ||
output_params(parameter_default_values, args.format, args.sort) | ||
|
||
|
||
if __name__ == "__main__": | ||
main() |
176 changes: 176 additions & 0 deletions
176
Tools/autotest/param_metadata/extract_param_defaults_unittest.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,176 @@ | ||
#!/usr/bin/python3 | ||
|
||
''' | ||
Extracts parameter default values from an ArduPilot .bin file. Unittests. | ||
AP_FLAKE8_CLEAN | ||
Amilcar do Carmo Lucas, IAV GmbH | ||
''' | ||
|
||
import unittest | ||
from unittest.mock import patch, MagicMock | ||
from extract_param_defaults import extract_parameter_default_values, missionplanner_sort, mavproxy_sort, output_params | ||
|
||
|
||
class TestExtractParameterDefaultValues(unittest.TestCase): | ||
|
||
@patch('extract_param_defaults.mavutil.mavlink_connection') | ||
def test_extract_parameter_default_values(self, mock_mavlink_connection): | ||
# Mock the mavlink connection and the messages it returns | ||
mock_mlog = MagicMock() | ||
mock_mavlink_connection.return_value = mock_mlog | ||
mock_mlog.recv_match.side_effect = [ | ||
MagicMock(Name='PARAM1', Default=1.1), | ||
MagicMock(Name='PARAM2', Default=2.0), | ||
None # End of messages | ||
] | ||
|
||
# Call the function with a dummy logfile path | ||
defaults = extract_parameter_default_values('dummy.bin') | ||
|
||
# Check if the defaults dictionary contains the correct parameters and values | ||
self.assertEqual(defaults, {'PARAM1': 1.1, 'PARAM2': 2.0}) | ||
|
||
@patch('extract_param_defaults.mavutil.mavlink_connection') | ||
def test_no_parameters(self, mock_mavlink_connection): | ||
# Mock the mavlink connection to return no parameter messages | ||
mock_mlog = MagicMock() | ||
mock_mavlink_connection.return_value = mock_mlog | ||
mock_mlog.recv_match.return_value = None | ||
|
||
# Call the function with a dummy logfile path | ||
defaults = extract_parameter_default_values('dummy.bin') | ||
|
||
# Check if the defaults dictionary is empty | ||
self.assertEqual(defaults, {}) | ||
|
||
@patch('extract_param_defaults.mavutil.mavlink_connection') | ||
def test_no_parameter_defaults(self, mock_mavlink_connection): | ||
# Mock the mavlink connection to simulate no parameter default values in the .bin file | ||
mock_mlog = MagicMock() | ||
mock_mavlink_connection.return_value = mock_mlog | ||
mock_mlog.recv_match.return_value = None # No PARM messages | ||
|
||
# Capture the output to check for the expected print statement | ||
with patch('builtins.print') as mock_print: | ||
# Call the function with a dummy logfile path | ||
defaults = extract_parameter_default_values('dummy.bin') | ||
|
||
# Check if the defaults dictionary is empty | ||
self.assertEqual(defaults, {}) | ||
|
||
# Check if the correct message was printed | ||
mock_print.assert_called_once_with("The .bin file contained no parameter default values. " | ||
"Update to a newer ArduPilot firmware version") | ||
|
||
@patch('extract_param_defaults.mavutil.mavlink_connection') | ||
def test_invalid_parameter_name(self, mock_mavlink_connection): | ||
# Mock the mavlink connection to simulate an invalid parameter name | ||
mock_mlog = MagicMock() | ||
mock_mavlink_connection.return_value = mock_mlog | ||
mock_mlog.recv_match.return_value = MagicMock(Name='INVALID_NAME%', Default=1.0) | ||
|
||
# Call the function with a dummy logfile path | ||
with self.assertRaises(SystemExit): | ||
extract_parameter_default_values('dummy.bin') | ||
|
||
@patch('extract_param_defaults.mavutil.mavlink_connection') | ||
def test_long_parameter_name(self, mock_mavlink_connection): | ||
# Mock the mavlink connection to simulate a too long parameter name | ||
mock_mlog = MagicMock() | ||
mock_mavlink_connection.return_value = mock_mlog | ||
mock_mlog.recv_match.return_value = MagicMock(Name='TOO_LONG_PARAMETER_NAME', Default=1.0) | ||
|
||
# Call the function with a dummy logfile path | ||
with self.assertRaises(SystemExit): | ||
extract_parameter_default_values('dummy.bin') | ||
|
||
|
||
class TestSortFunctions(unittest.TestCase): | ||
def test_missionplanner_sort(self): | ||
# Define a list of parameter names | ||
params = ['PARAM_GROUP1_PARAM1', 'PARAM_GROUP2_PARAM2', 'PARAM_GROUP1_PARAM2'] | ||
|
||
# Sort the parameters using the missionplanner_sort function | ||
sorted_params = sorted(params, key=missionplanner_sort) | ||
|
||
# Check if the parameters were sorted correctly | ||
self.assertEqual(sorted_params, ['PARAM_GROUP1_PARAM1', 'PARAM_GROUP1_PARAM2', 'PARAM_GROUP2_PARAM2']) | ||
|
||
# Test with a parameter name that doesn't contain an underscore | ||
params = ['PARAM1', 'PARAM3', 'PARAM2'] | ||
sorted_params = sorted(params, key=missionplanner_sort) | ||
self.assertEqual(sorted_params, ['PARAM1', 'PARAM2', 'PARAM3']) | ||
|
||
def test_mavproxy_sort(self): | ||
# Define a list of parameter names | ||
params = ['PARAM_GROUP1_PARAM1', 'PARAM_GROUP2_PARAM2', 'PARAM_GROUP1_PARAM2'] | ||
|
||
# Sort the parameters using the mavproxy_sort function | ||
sorted_params = sorted(params, key=mavproxy_sort) | ||
|
||
# Check if the parameters were sorted correctly | ||
self.assertEqual(sorted_params, ['PARAM_GROUP1_PARAM1', 'PARAM_GROUP1_PARAM2', 'PARAM_GROUP2_PARAM2']) | ||
|
||
# Test with a parameter name that doesn't contain an underscore | ||
params = ['PARAM1', 'PARAM3', 'PARAM2'] | ||
sorted_params = sorted(params, key=mavproxy_sort) | ||
self.assertEqual(sorted_params, ['PARAM1', 'PARAM2', 'PARAM3']) | ||
|
||
|
||
class TestOutputParams(unittest.TestCase): | ||
|
||
@patch('extract_param_defaults.print') | ||
def test_output_params(self, mock_print): | ||
# Prepare a dummy defaults dictionary | ||
defaults = {'PARAM1': 1.0, 'PARAM2': 2.0} | ||
|
||
# Call the function with the dummy dictionary, 'missionplanner' format type and 'none' sort type | ||
output_params(defaults, 'missionplanner', 'none') | ||
|
||
# Check if the print function was called with the correct parameters | ||
expected_calls = [unittest.mock.call('PARAM1,1'), unittest.mock.call('PARAM2,2')] | ||
mock_print.assert_has_calls(expected_calls, any_order=True) | ||
|
||
@patch('extract_param_defaults.print') | ||
def test_output_params_mavproxy(self, mock_print): | ||
# Prepare a dummy defaults dictionary | ||
defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} | ||
|
||
# Call the function with the dummy dictionary, 'mavproxy' format type and 'mavproxy' sort type | ||
output_params(defaults, 'mavproxy', '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))] | ||
mock_print.assert_has_calls(expected_calls, any_order=True) | ||
|
||
@patch('extract_param_defaults.print') | ||
def test_output_params_qgcs(self, mock_print): | ||
# Prepare a dummy defaults dictionary | ||
defaults = {'PARAM2': 2.0, 'PARAM1': 1.0} | ||
|
||
# Call the function with the dummy dictionary, 'qgcs' format type and 'qgcs' sort type | ||
output_params(defaults, 'qgcs', 'qgcs') | ||
|
||
# Check if the print function was called with the correct parameters | ||
expected_calls = [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))] | ||
mock_print.assert_has_calls(expected_calls, any_order=True) | ||
|
||
@patch('extract_param_defaults.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} | ||
|
||
# Call the function with the dummy dictionary, 'missionplanner' format type and 'missionplanner' sort type | ||
output_params(defaults, 'missionplanner', 'missionplanner') | ||
|
||
# Check if the print function was called with the correct parameters | ||
expected_calls = [unittest.mock.call('PARAM1,1.01'), unittest.mock.call('PARAM2,2')] | ||
mock_print.assert_has_calls(expected_calls, any_order=True) | ||
|
||
|
||
if __name__ == '__main__': | ||
unittest.main() |