def test_endpoint_validators() -> None:
with pytest.raises(ValueError):
Endpoint.is_mavlink_endpoint({
"connection_type": EndpointType.UDPServer,
"place": "0.0.0.0",
"argument": -30
})
with pytest.raises(ValueError):
Endpoint.is_mavlink_endpoint({
"connection_type": EndpointType.UDPServer,
"place": "42",
"argument": 14555
})
with pytest.raises(ValueError):
Endpoint.is_mavlink_endpoint({
"connection_type": EndpointType.Serial,
"place": "dev/autopilot",
"argument": 115200
})
with pytest.raises(ValueError):
Endpoint.is_mavlink_endpoint({
"connection_type": EndpointType.Serial,
"place": serial_port_name,
"argument": 100000
})
with pytest.raises(ValueError):
Endpoint.is_mavlink_endpoint({
"connection_type": "potato",
"place": serial_port_name,
"argument": 100
})
def start_mavlink_manager(self, device: Endpoint) -> None:
default_endpoints = [
Endpoint(
"GCS Server Link",
self.settings.app_name,
EndpointType.UDPServer,
"0.0.0.0",
14550,
persistent=True,
enabled=False,
),
Endpoint(
"GCS Client Link",
self.settings.app_name,
EndpointType.UDPClient,
"192.168.2.1",
14550,
persistent=True,
enabled=True,
),
Endpoint(
"MAVLink2Rest",
self.settings.app_name,
EndpointType.UDPClient,
"127.0.0.1",
14000,
persistent=True,
protected=True,
),
Endpoint(
"Internal Link",
self.settings.app_name,
EndpointType.UDPServer,
"127.0.0.1",
14001,
persistent=True,
protected=True,
),
Endpoint(
"Ping360 Heading",
self.settings.app_name,
EndpointType.UDPServer,
"0.0.0.0",
14660,
persistent=True,
protected=True,
),
]
for endpoint in default_endpoints:
try:
self.mavlink_manager.add_endpoint(endpoint)
except EndpointAlreadyExists:
pass
except Exception as error:
logger.warning(str(error))
self.mavlink_manager.start(device)
def valid_master_endpoints() -> Set[Endpoint]:
return {
Endpoint("Master endpoint 1", "pytest", EndpointType.UDPServer,
"0.0.0.0", 14550),
Endpoint("Master endpoint 2", "pytest", EndpointType.UDPClient,
"0.0.0.0", 14550),
Endpoint("Master endpoint 3", "pytest", EndpointType.TCPServer,
"0.0.0.0", 14550),
Endpoint("Master endpoint 4", "pytest", EndpointType.TCPClient,
"0.0.0.0", 14550),
Endpoint("Master endpoint 5", "pytest", EndpointType.Serial,
serial_port_name, 115200),
}
def valid_output_endpoints() -> Set[Endpoint]:
return {
Endpoint("Test endpoint 1", "pytest", EndpointType.UDPServer,
"0.0.0.0", 14551),
Endpoint("Test endpoint 2", "pytest", EndpointType.UDPClient,
"0.0.0.0", 14552),
Endpoint("Test endpoint 3", "pytest", EndpointType.TCPServer,
"0.0.0.0", 14553),
Endpoint("Test endpoint 4", "pytest", EndpointType.TCPClient,
"0.0.0.0", 14554),
Endpoint("Test endpoint 5", "pytest", EndpointType.Serial,
serial_port_name, 57600),
}
def start_sitl(self, frame: SITLFrame = SITLFrame.VECTORED) -> None:
self._current_board = BoardDetector.detect_sitl()
if not self.firmware_manager.is_firmware_installed(self._current_board):
self.firmware_manager.install_firmware_from_params(Vehicle.Sub, self._current_board)
if frame == SITLFrame.UNDEFINED:
frame = SITLFrame.VECTORED
logger.warning(f"SITL frame is undefined. Setting {frame} as current frame.")
self.current_sitl_frame = frame
firmware_path = self.firmware_manager.firmware_path(self._current_board.platform)
self.firmware_manager.validate_firmware(firmware_path, self._current_board.platform)
# ArduPilot SITL binary will bind TCP port 5760 (server) and the mavlink router will connect to it as a client
master_endpoint = Endpoint(
"Master", self.settings.app_name, EndpointType.TCPServer, "127.0.0.1", 5760, protected=True
)
# pylint: disable=consider-using-with
self.ardupilot_subprocess = subprocess.Popen(
[
firmware_path,
"--model",
self.current_sitl_frame.value,
"--base-port",
str(master_endpoint.argument),
"--home",
"-27.563,-48.459,0.0,270.0",
],
shell=False,
encoding="utf-8",
errors="ignore",
cwd=self.settings.firmware_folder,
)
self.start_mavlink_manager(master_endpoint)
def start_serial(self, board: FlightController) -> None:
if not board.path:
raise ValueError(f"Could not find device path for board {board.name}.")
self._current_board = board
self.start_mavlink_manager(
Endpoint("Master", self.settings.app_name, EndpointType.Serial, board.path, 115200, protected=True)
)
def assemble_command(self) -> str:
if self._master_endpoint is None:
raise NoMasterMavlinkEndpoint(
"Mavlink master endpoint was not set. Cannot proceed.")
serial_endpoint_as_input: Callable[
[Endpoint],
str] = lambda endpoint: f"{endpoint.place},{endpoint.argument}"
# Convert endpoint format to mavproxy format
def convert_endpoint(endpoint: Endpoint) -> str:
if endpoint.connection_type != EndpointType.Serial:
return f"--out={str(endpoint)}"
return f"--out={serial_endpoint_as_input(endpoint)}"
filtered_endpoints = Endpoint.filter_enabled(self.endpoints())
endpoints = " ".join(
[convert_endpoint(endpoint) for endpoint in filtered_endpoints])
master = self._master_endpoint
master_string = str(master)
if master.connection_type == EndpointType.Serial:
if not master.argument:
master_string = f"{master.place}"
else:
master_string = f"{serial_endpoint_as_input(master)}"
if master.connection_type == EndpointType.TCPServer:
master_string = f"tcp:{master.place}:{master.argument}"
log = f"--state-basedir={self.logdir().resolve()}"
return f"{self.binary()} --master={master_string} {endpoints} {log} --non-interactive"
def start_navigator(self, board: FlightController) -> None:
self.current_platform = board.platform
if not self.firmware_manager.is_firmware_installed(
self.current_platform):
if board.platform == Platform.NavigatorR3:
self.firmware_manager.install_firmware_from_params(
Vehicle.Sub, self.current_platform)
else:
self.install_firmware_from_file(
pathlib.Path(
"/root/companion-files/ardupilot-manager/default/ardupilot_navigator_r4"
))
self.firmware_manager.validate_firmware(self.current_firmware_path(),
self.current_platform)
# ArduPilot process will connect as a client on the UDP server created by the mavlink router
master_endpoint = Endpoint("Master",
self.settings.app_name,
EndpointType.UDPServer,
"127.0.0.1",
8852,
protected=True)
# Run ardupilot inside while loop to avoid exiting after reboot command
## Can be changed back to a simple command after https://github.com/ArduPilot/ardupilot/issues/17572
## gets fixed.
# pylint: disable=consider-using-with
#
# The mapping of serial ports works as in the following table:
#
# | ArduSub | Navigator |
# | -C = Serial1 | Serial1 => /dev/ttyS0 |
# | -B = Serial3 | Serial3 => /dev/ttyAMA1 |
# | -E = Serial4 | Serial4 => /dev/ttyAMA2 |
# | -F = Serial5 | Serial5 => /dev/ttyAMA3 |
#
# The first column comes from https://ardupilot.org/dev/docs/sitl-serial-mapping.html
self.ardupilot_subprocess = subprocess.Popen(
f"{self.current_firmware_path()}"
f" -A udp:{master_endpoint.place}:{master_endpoint.argument}"
f" --log-directory {self.settings.firmware_folder}/logs/"
f" --storage-directory {self.settings.firmware_folder}/storage/"
f" -C /dev/ttyS0"
f" -B /dev/ttyAMA1"
f" -E /dev/ttyAMA2"
f" -F /dev/ttyAMA3",
shell=True,
encoding="utf-8",
errors="ignore",
)
self.start_mavlink_manager(master_endpoint)
def assemble_command(self) -> str:
if self._master_endpoint is None:
raise NoMasterMavlinkEndpoint(
"Mavlink master endpoint was not set. Cannot proceed.")
# Convert endpoint format to mavlink-router format
def convert_endpoint(endpoint: Endpoint) -> str:
# TCP uses a special argument and only works with localhost
if endpoint.connection_type == EndpointType.TCPServer:
return f"--tcp-port {endpoint.argument}"
if endpoint.connection_type == EndpointType.TCPClient:
return f"--tcp-endpoint {endpoint.place}:{endpoint.argument}"
if endpoint.connection_type == EndpointType.UDPServer:
return f"{endpoint.place}:{endpoint.argument}"
if endpoint.connection_type == EndpointType.UDPClient:
return f"--endpoint {endpoint.place}:{endpoint.argument}"
raise ValueError(
f"Endpoint of type {endpoint.connection_type} not supported on MavlinkRouter."
)
# Since MAVLinkRouter does not have an argument specifier for UDP Server connections
# (e.g. "--endpoint" for UDP Clients), we rely on the UDP Server endpoints being
# on the beginning of the endpoints list, and so as the first endpoints on the
# process call, for them to work
types_order = {
EndpointType.UDPServer: 0,
EndpointType.UDPClient: 1,
EndpointType.TCPServer: 2,
EndpointType.TCPClient: 3,
}
sorted_endpoints = sorted(
self.endpoints(),
key=lambda endpoint: types_order[endpoint.connection_type
] # type: ignore
)
filtered_endpoints = Endpoint.filter_enabled(sorted_endpoints)
endpoints = " ".join(
[convert_endpoint(endpoint) for endpoint in filtered_endpoints])
master = self._master_endpoint
if master.connection_type not in [
EndpointType.UDPServer,
EndpointType.Serial,
EndpointType.TCPServer,
]:
raise ValueError(
f"Master endpoint of type {master.connection_type} not supported on MavlinkRouter."
)
if master.connection_type == EndpointType.TCPServer:
# The "--tcp-port 0" argument is used to prevent the router from binding TCP port 5760
return f"{self.binary()} --tcp-endpoint {master.place}:{master.argument} --tcp-port 0 {endpoints}"
return f"{self.binary()} {master.place}:{master.argument} --tcp-port 0 {endpoints}"
def test_endpoint() -> None:
endpoint = Endpoint("Test endpoint", "pytest", EndpointType.UDPClient,
"0.0.0.0", 14550)
assert endpoint.name == "Test endpoint", "Name does not match."
assert endpoint.owner == "pytest", "Owner does not match."
assert endpoint.persistent is False, "Persistent does not match."
assert endpoint.protected is False, "Protected does not match."
assert endpoint.connection_type == EndpointType.UDPClient, "Connection type does not match."
assert endpoint.place == "0.0.0.0", "Connection place does not match."
assert endpoint.argument == 14550, "Connection argument does not match."
assert endpoint.__str__(
) == "udpout:0.0.0.0:14550", "Connection string does not match."
assert endpoint.as_dict() == {
"name": "Test endpoint",
"owner": "pytest",
"connection_type": EndpointType.UDPClient,
"place": "0.0.0.0",
"argument": 14550,
"persistent": False,
"protected": False,
"enabled": True,
}, "Endpoint dict does not match."
def _get_configuration_endpoints(self) -> Set[Endpoint]:
return {
Endpoint(**endpoint)
for endpoint in self.configuration.get("endpoints") or []
}
def start_mavlink_manager(self, device: Endpoint) -> None:
self.mavlink_manager.add_endpoints([Endpoint("udpin:0.0.0.0:14550")])
self.mavlink_manager.set_master_endpoint(device)
self.mavlink_manager.start()
while self.mavlink_manager.is_running():
time.sleep(1)
def start_serial(self, device: str) -> None:
self.start_mavlink_manager(Endpoint(f"serial:{device}:115200"))