Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
19 changes: 12 additions & 7 deletions ardupilot_methodic_configurator/__main__.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
from ardupilot_methodic_configurator.backend_flightcontroller import FlightController
from ardupilot_methodic_configurator.backend_internet import verify_and_open_url
from ardupilot_methodic_configurator.common_arguments import add_common_arguments
from ardupilot_methodic_configurator.data_model_par_dict import ParDict
from ardupilot_methodic_configurator.data_model_software_updates import UpdateManager, check_for_software_updates
from ardupilot_methodic_configurator.data_model_vehicle_project import VehicleProjectManager
from ardupilot_methodic_configurator.frontend_tkinter_component_editor import ComponentEditorWindow
Expand All @@ -53,7 +54,7 @@ def __init__(self, args: argparse.Namespace) -> None:
self.args = args
self.flight_controller: FlightController = None # type: ignore[assignment]
self.vehicle_type: str = ""
self.param_default_values: dict = {}
self.param_default_values: ParDict = ParDict()
self.local_filesystem: LocalFilesystem = None # type: ignore[assignment]
self.vehicle_project_manager: Union[VehicleProjectManager, None] = None
self.param_default_values_dirty: bool = False
Expand Down Expand Up @@ -167,7 +168,8 @@ def initialize_flight_controller_and_filesystem(state: ApplicationState) -> None
# Get default parameter values from flight controller
if state.flight_controller.master is not None or state.args.device == "test":
fciw = FlightControllerInfoWindow(state.flight_controller)
state.param_default_values = fciw.get_param_default_values()
default_values = fciw.get_param_default_values()
state.param_default_values = ParDict(default_values) if default_values else ParDict()

# Initialize local filesystem
try:
Expand Down Expand Up @@ -218,7 +220,8 @@ def vehicle_directory_selection(state: ApplicationState) -> Union[VehicleDirecto
state.flight_controller.reset_and_reconnect()
if state.flight_controller.master is not None or state.args.device == "test":
fciw = FlightControllerInfoWindow(state.flight_controller)
state.param_default_values = fciw.get_param_default_values()
default_values = fciw.get_param_default_values()
state.param_default_values = ParDict(default_values) if default_values else ParDict()

return vehicle_dir_window

Expand Down Expand Up @@ -474,10 +477,12 @@ def parameter_editor_and_uploader(state: ApplicationState) -> None:
}
if state.flight_controller.fc_parameters and state.flight_controller.info.flight_sw_version.startswith("4.6."):
for filename in state.local_filesystem.file_parameters:
state.local_filesystem.file_parameters[filename] = {
param_upgrade_dict.get(parameter_name, parameter_name): par
for parameter_name, par in state.local_filesystem.file_parameters[filename].items()
}
state.local_filesystem.file_parameters[filename] = ParDict(
{
param_upgrade_dict.get(parameter_name, parameter_name): par
for parameter_name, par in state.local_filesystem.file_parameters[filename].items()
}
)

# Call the GUI function with the starting intermediate parameter file
ParameterEditorWindow(start_file, state.flight_controller, state.local_filesystem)
Expand Down
259 changes: 15 additions & 244 deletions ardupilot_methodic_configurator/annotate_params.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,26 +30,22 @@
import re
from os import environ as os_environ
from os import path as os_path
from os import popen as os_popen
from sys import exc_info as sys_exc_info
from sys import exit as sys_exit
from types import TracebackType
from typing import Any, Optional, Union
from xml.etree import ElementTree as ET # no parsing, just data-structure manipulation

import argcomplete
from argcomplete.completers import FilesCompleter
from defusedxml import ElementTree as DET # noqa: N814, just parsing, no data-structure manipulation

from ardupilot_methodic_configurator.data_model_par_dict import PARAM_NAME_MAX_LEN, PARAM_NAME_REGEX, ParDict

# URL of the XML file
BASE_URL = "https://autotest.ardupilot.org/Parameters/"

PARAM_DEFINITION_XML_FILE = "apm.pdef.xml"
LUA_PARAM_DEFINITION_XML_FILE = "24_inflight_magnetometer_fit_setup.pdef.xml"

# ArduPilot parameter names start with a capital letter and can have capital letters, numbers and _
PARAM_NAME_REGEX = r"^[A-Z][A-Z_0-9]*"
PARAM_NAME_MAX_LEN = 16
VERSION = "1.0"

# mypy: disable-error-code="unused-ignore"
Expand Down Expand Up @@ -137,235 +133,6 @@ def check_max_line_length(value: int) -> int:
return args


class Par:
"""
Represents a parameter with a value and an optional comment.

Attributes:
value (float): The value of the parameter.
comment (Optional[str]): An optional comment associated with the parameter.

"""

def __init__(self, value: float, comment: Optional[str] = None) -> None:
self.value = value
self.comment = comment

def __eq__(self, other: object) -> bool:
"""Equality operation."""
if isinstance(other, Par):
return self.value == other.value and self.comment == other.comment
return False

def __hash__(self) -> int:
"""Hash operation for using Par objects in sets and as dict keys."""
return hash((self.value, self.comment))

@staticmethod
def load_param_file_into_dict(param_file: str) -> dict[str, "Par"]:
"""
Loads an ArduPilot parameter file into a dictionary with name, value pairs.

Args:
param_file (str): The name of the parameter file to load.

Returns:
dict: A dictionary containing the parameters from the file.

"""
parameter_dict: dict[str, Par] = {}
try:
with open(param_file, encoding="utf-8") as f_handle:
for i, f_line in enumerate(f_handle, start=1):
original_line = f_line
line = f_line.strip()
comment = None
if not line:
continue # skip empty lines
if line[0] == "#":
continue # skip comments
if "#" in line:
line, comment = line.split("#", 1) # strip trailing comments
comment = comment.strip()
if "," in line:
# parse mission planner style parameter files
parameter, value = line.split(",", 1)
elif " " in line:
# parse mavproxy style parameter files
parameter, value = line.split(" ", 1)
elif "\t" in line:
parameter, value = line.split("\t", 1)
else:
msg = f"Missing parameter-value separator: {line} in {param_file} line {i}"
raise SystemExit(msg)
parameter = parameter.strip()
Par.validate_parameter(param_file, parameter_dict, i, original_line, comment, parameter, value)
except UnicodeDecodeError as exp:
msg = f"Fatal error reading {param_file}: {exp}"
raise SystemExit(msg) from exp
return parameter_dict

@staticmethod
def validate_parameter( # pylint: disable=too-many-arguments, too-many-positional-arguments
param_file: str,
parameter_dict: dict[str, "Par"],
i: int,
original_line: str,
comment: Union[None, str],
parameter_name: str,
value: str,
) -> None:
if len(parameter_name) > PARAM_NAME_MAX_LEN:
msg = f"Too long parameter name: {parameter_name} in {param_file} line {i}"
raise SystemExit(msg)
if not re.match(PARAM_NAME_REGEX, parameter_name):
msg = f"Invalid characters in parameter name {parameter_name} in {param_file} line {i}"
raise SystemExit(msg)
if parameter_name in parameter_dict:
msg = f"Duplicated parameter {parameter_name} in {param_file} line {i}"
raise SystemExit(msg)
try:
fvalue = float(value.strip())
parameter_dict[parameter_name] = Par(fvalue, comment)
except ValueError as exc:
msg = f"Invalid parameter value {value} in {param_file} line {i}"
raise SystemExit(msg) from exc
except OSError as exc:
_exc_type, exc_value, exc_traceback = sys_exc_info()
if isinstance(exc_traceback, TracebackType):
fname = os_path.split(exc_traceback.tb_frame.f_code.co_filename)[1]
logging.critical("in line %s of file %s: %s", exc_traceback.tb_lineno, fname, exc_value)
msg = f"Caused by line {i} of file {param_file}: {original_line}"
raise SystemExit(msg) from exc

@staticmethod
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)

@staticmethod
def format_params(param_dict: dict[str, "Par"], file_format: str = "missionplanner") -> list[str]: # pylint: disable=too-many-branches
"""
Formats the parameters in the provided dictionary into a list of strings.

Each string in the returned list is a formatted representation of a parameter,
consisting of the parameter's name, its value, and optionally its comment.
The comment is included if it is present in the parameter's 'Par' object.

Args:
param_dict (Dict[str, 'Par']): A dictionary of 'Par' objects.
Each key is a parameter name and each value is a 'Par' object.
Par can be a simple float or a Par object with a comment.
file_format (str): Can be "missionplanner" or "mavproxy"

Returns:
List[str]: A list of strings, each string representing a parameter from the input dictionary
in the format "name,value # comment".

"""
if file_format == "missionplanner":
param_dict = dict(sorted(param_dict.items(), key=lambda x: Par.missionplanner_sort(x[0]))) # sort alphabetically
elif file_format == "mavproxy":
param_dict = dict(sorted(param_dict.items())) # sort in ASCIIbetical order
else:
msg = f"ERROR: Unsupported file format {file_format}"
raise SystemExit(msg)

formatted_params = []
if file_format == "missionplanner":
for key, parameter in param_dict.items():
if isinstance(parameter, Par):
if parameter.comment:
formatted_params.append(
f"{key},{format(parameter.value, '.6f').rstrip('0').rstrip('.')} # {parameter.comment}"
)
else:
formatted_params.append(f"{key},{format(parameter.value, '.6f').rstrip('0').rstrip('.')}")
else:
formatted_params.append(f"{key},{format(parameter, '.6f').rstrip('0').rstrip('.')}")
elif file_format == "mavproxy":
for key, parameter in param_dict.items():
if isinstance(parameter, Par):
if parameter.comment:
formatted_params.append(f"{key:<16} {parameter.value:<8.6f} # {parameter.comment}")
else:
formatted_params.append(f"{key:<16} {parameter.value:<8.6f}")
else:
formatted_params.append(f"{key:<16} {parameter:<8.6f}")
return formatted_params

@staticmethod
def export_to_param(formatted_params: list[str], filename_out: str) -> None:
"""
Exports a list of formatted parameters to an ArduPilot parameter file.

Args:
formatted_params (List[str]): The list of formatted parameters to export.
filename_out (str): The output filename.

Returns:
None

"""
if not formatted_params:
return
try:
# Ensure newline character is LF, even on windows
with open(filename_out, "w", encoding="utf-8", newline="\n") as output_file:
output_file.writelines(line + "\n" for line in formatted_params)
except OSError as e:
msg = f"ERROR: writing to file {filename_out}: {e}"
raise SystemExit(msg) from e

@staticmethod
def print_out(formatted_params: list[str], name: str) -> None:
"""
Print out the contents of the provided list.

If the list is too large, print only the ones that fit on the screen and
wait for user input to continue.

Args:
formatted_params (List[str]): The list of formatted parameters to print.
name (str): A descriptive string for the list contents

Returns:
None

"""
if not formatted_params:
return

rows_str = "100" # number of lines to display before waiting for user input

# Get the size of the terminal
if __name__ == "__main__":
rows_str, _columns = os_popen("stty size", "r").read().split() # noqa: S605, S607

# Convert rows to integer
rows = int(rows_str) - 2 # -2 for the next print and the input line

# Convert rows
print(f"\n{name} has {len(formatted_params)} parameters:") # noqa: T201
for i, line in enumerate(formatted_params):
if i % rows == 0 and __name__ == "__main__":
input(f"\n{name} list is long hit enter to continue")
rows_str, _columns = os_popen("stty size", "r").read().split() # noqa: S605, S607
rows = int(rows_str) - 2 # -2 for the next print and the input line
print(line) # noqa: T201


def get_xml_data(base_url: str, directory: str, filename: str, vehicle_type: str) -> ET.Element: # pylint: disable=too-many-locals
"""
Fetches XML data from a local file or a URL.
Expand Down Expand Up @@ -461,11 +228,11 @@ def get_env_proxies() -> Union[dict[str, str], None]:
return proxies


def load_default_param_file(directory: str) -> dict[str, "Par"]:
param_default_dict: dict[str, Par] = {}
def load_default_param_file(directory: str) -> ParDict:
param_default_dict: ParDict = ParDict()
# Load parameter default values if the 00_default.param file exists
try:
param_default_dict = Par.load_param_file_into_dict(os_path.join(directory, "00_default.param"))
param_default_dict = ParDict.from_file(os_path.join(directory, "00_default.param"))
except FileNotFoundError:
logging.warning("Default parameter file 00_default.param not found. No default values will be annotated.")
logging.warning("Create one by using the command ./extract_param_defaults.py log_file.bin > 00_default.param")
Expand Down Expand Up @@ -647,20 +414,24 @@ def extract_parameter_name_and_validate(line: str, filename: str, line_nr: int)
SystemExit: If the line is invalid or the parameter name is too long or invalid.

"""
# Extract the parameter name
match = re.match(PARAM_NAME_REGEX, line)
# Extract the parameter name from the line (until we hit a separator)
# Create a regex to extract parameter name followed by separator
param_line_pattern = r"^([A-Z][A-Z_0-9]*)[,\s\t]"
match = re.match(param_line_pattern, line)
if match:
param_name = match.group(0)
param_name = match.group(1)
else:
logging.critical("Invalid line %d in file %s: %s", line_nr, filename, line)
msg = "Invalid line in input file"
raise SystemExit(msg)
param_len = len(param_name)
param_sep = line[param_len] # the character following the parameter name must be a separator
if param_sep not in {",", " ", "\t"}:

# Validate the extracted parameter name against the strict parameter name regex
if not re.match(PARAM_NAME_REGEX, param_name):
logging.critical("Invalid parameter name %s on line %d in file %s", param_name, line_nr, filename)
msg = "Invalid parameter name"
raise SystemExit(msg)

param_len = len(param_name)
if param_len > PARAM_NAME_MAX_LEN:
logging.critical("Too long parameter name on line %d in file %s", line_nr, filename)
msg = "Too long parameter name"
Expand Down
Loading
Loading