Skip to content

Commit c4f79f8

Browse files
committed
feat(flight controller): Support configurable FC baudrate
Closes #538
1 parent 534966c commit c4f79f8

File tree

6 files changed

+51
-31
lines changed

6 files changed

+51
-31
lines changed

ardupilot_methodic_configurator/__main__.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,7 @@ def create_argument_parser() -> argparse.ArgumentParser:
7979

8080

8181
def connect_to_fc_and_set_vehicle_type(args: argparse.Namespace) -> tuple[FlightController, str]:
82-
flight_controller = FlightController(args.reboot_time)
82+
flight_controller = FlightController(reboot_time=args.reboot_time, baudrate=args.baudrate)
8383

8484
error_str = flight_controller.connect(args.device, log_errors=False)
8585
if error_str:

ardupilot_methodic_configurator/backend_flightcontroller.py

Lines changed: 26 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,10 @@ def close(self) -> None:
6060
pass
6161

6262

63+
DEFAULT_BAUDRATE: int = 115200
64+
DEFAULT_REBOOT_TIME: int = 7
65+
66+
6367
class FlightController:
6468
"""
6569
A class to manage the connection and parameters of a flight controller.
@@ -71,13 +75,14 @@ class FlightController:
7175
7276
"""
7377

74-
def __init__(self, reboot_time: int) -> None:
78+
def __init__(self, reboot_time: int = DEFAULT_REBOOT_TIME, baudrate: int = DEFAULT_BAUDRATE) -> None:
7579
"""Initialize the FlightController communication object."""
7680
# warn people about ModemManager which interferes badly with ArduPilot
7781
if os_path.exists("/usr/sbin/ModemManager"):
7882
logging_warning(_("You should uninstall ModemManager as it conflicts with ArduPilot"))
7983

8084
self.__reboot_time = reboot_time
85+
self.__baudrate = baudrate
8186
self.__connection_tuples: list[tuple[str, str]] = []
8287
self.discover_connections()
8388
self.master: Union[mavutil.mavlink_connection, None] = None # pyright: ignore[reportGeneralTypeIssues]
@@ -167,7 +172,9 @@ def connect(
167172
self.__connection_tuples.insert(-1, (self.comport.device, getattr(self.comport, "description", "")))
168173
else:
169174
return _("No serial ports found. Please connect a flight controller and try again.")
170-
return self.__create_connection_with_retry(progress_callback=progress_callback, log_errors=log_errors)
175+
return self.__create_connection_with_retry(
176+
progress_callback=progress_callback, baudrate=self.__baudrate, log_errors=log_errors
177+
)
171178

172179
def __request_banner(self) -> None:
173180
"""Request banner information from the flight controller."""
@@ -219,11 +226,12 @@ def __request_message(self, message_id: int) -> None:
219226
0,
220227
)
221228

222-
def __create_connection_with_retry(
229+
def __create_connection_with_retry( # pylint: disable=too-many-arguments, too-many-positional-arguments
223230
self,
224231
progress_callback: Union[None, Callable[[int, int], None]],
225232
retries: int = 3,
226233
timeout: int = 5,
234+
baudrate: int = DEFAULT_BAUDRATE,
227235
log_errors: bool = True,
228236
) -> str:
229237
"""
@@ -239,6 +247,7 @@ def __create_connection_with_retry(
239247
of the connection attempt. Default is None.
240248
retries (int, optional): The number of retries before giving up. Default is 3.
241249
timeout (int, optional): The timeout in seconds for each connection attempt. Default is 5.
250+
baudrate (int, optional): The baud rate for the connection. Default is DEFAULT_BAUDRATE.
242251
log_errors (bool): log errors.
243252
244253
Returns:
@@ -248,11 +257,15 @@ def __create_connection_with_retry(
248257
"""
249258
if self.comport is None or self.comport.device == "test": # FIXME for testing only pylint: disable=fixme
250259
return ""
251-
logging_info(_("Will connect to %s"), self.comport.device)
260+
logging_info(_("Will connect to %s @ %u baud"), self.comport.device, baudrate)
252261
try:
253262
# Create the connection
254263
self.master = mavutil.mavlink_connection(
255-
device=self.comport.device, timeout=timeout, retries=retries, progress_callback=progress_callback
264+
device=self.comport.device,
265+
baud=baudrate,
266+
timeout=timeout,
267+
retries=retries,
268+
progress_callback=progress_callback,
256269
)
257270
logging_debug(_("Waiting for MAVLink heartbeat"))
258271
if not self.master:
@@ -522,7 +535,7 @@ def reset_and_reconnect(
522535
reset_progress_callback(current_step, sleep_time)
523536

524537
# Reconnect to the flight controller
525-
return self.__create_connection_with_retry(connection_progress_callback)
538+
return self.__create_connection_with_retry(connection_progress_callback, baudrate=self.__baudrate)
526539

527540
@staticmethod
528541
def __list_serial_ports() -> list[serial.tools.list_ports_common.ListPortInfo]:
@@ -603,6 +616,12 @@ def put_progress_callback(completion: float) -> None:
603616

604617
@staticmethod
605618
def add_argparse_arguments(parser: ArgumentParser) -> ArgumentParser:
619+
parser.add_argument(
620+
"--baudrate",
621+
type=int,
622+
default=DEFAULT_BAUDRATE,
623+
help=_("MAVLink serial connection baudrate to the flight controller. Default is %(default)s"),
624+
)
606625
parser.add_argument( # type: ignore[attr-defined]
607626
"--device",
608627
type=str,
@@ -620,7 +639,7 @@ def add_argparse_arguments(parser: ArgumentParser) -> ArgumentParser:
620639
min=5,
621640
max=50,
622641
action=CheckRange,
623-
default=7,
642+
default=DEFAULT_REBOOT_TIME,
624643
help=_("Flight controller reboot time. Default is %(default)s"),
625644
)
626645
return parser

ardupilot_methodic_configurator/frontend_tkinter_connection_selection.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -285,8 +285,8 @@ def main() -> None:
285285
)
286286
# pylint: enable=duplicate-code
287287

288-
flight_controller = FlightController(args.reboot_time) # Initialize your FlightController instance
289-
result = flight_controller.connect(device=args.device) # Connect to the flight controller
288+
flight_controller = FlightController(reboot_time=args.reboot_time, baudrate=args.baudrate)
289+
result = flight_controller.connect(device=args.device)
290290
if result:
291291
logging_warning(result)
292292
window = ConnectionSelectionWindow(flight_controller, result)

ardupilot_methodic_configurator/frontend_tkinter_parameter_editor.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -961,7 +961,7 @@ def argument_parser() -> Namespace:
961961

962962
logging_basicConfig(level=logging_getLevelName(args.loglevel), format="%(asctime)s - %(levelname)s - %(message)s")
963963

964-
fc = FlightController(args.reboot_time)
964+
fc = FlightController(reboot_time=args.reboot_time, baudrate=args.baudrate)
965965
filesystem = LocalFilesystem(
966966
args.vehicle_dir, args.vehicle_type, "", args.allow_editing_template_files, args.save_component_to_system_templates
967967
)

tests/test__main__.py

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ def test_connect_to_fc_and_read_parameters(self, mock_flight_controller) -> None
5252
mock_fc.info.vendor = "vendor"
5353
mock_fc.info.firmware_type = "type"
5454

55-
args = argparse.Namespace(device="test_device", vehicle_type="", reboot_time=5)
55+
args = argparse.Namespace(device="test_device", vehicle_type="", reboot_time=5, baudrate=115200)
5656
flight_controller, vehicle_type = connect_to_fc_and_set_vehicle_type(args)
5757
assert flight_controller == mock_fc
5858
assert vehicle_type == "quad"
@@ -115,7 +115,7 @@ def test_connect_to_fc_with_explicit_vehicle_type(self, mock_flight_controller)
115115
mock_fc.info.flight_sw_version_and_type = "v1.0"
116116

117117
# Set an explicit vehicle type
118-
args = argparse.Namespace(device="test_device", vehicle_type="plane", reboot_time=5)
118+
args = argparse.Namespace(device="test_device", vehicle_type="plane", reboot_time=5, baudrate=115200)
119119
flight_controller, vehicle_type = connect_to_fc_and_set_vehicle_type(args)
120120

121121
assert flight_controller == mock_fc
@@ -131,7 +131,7 @@ def test_connect_to_fc_with_connection_error(self, mock_flight_controller) -> No
131131
mock_window_instance = MagicMock()
132132
mock_window.return_value = mock_window_instance
133133

134-
args = argparse.Namespace(device="test_device", vehicle_type="", reboot_time=5)
134+
args = argparse.Namespace(device="test_device", vehicle_type="", reboot_time=5, baudrate=115200)
135135
connect_to_fc_and_set_vehicle_type(args)
136136

137137
# Verify ConnectionSelectionWindow was created with the right parameters
@@ -198,6 +198,7 @@ def test_backup_fc_parameters(self, mock_param_editor, mock_local_filesystem, mo
198198
# Just calling connect_to_fc_and_set_vehicle_type to get flight_controller
199199
args = argparse.Namespace(
200200
device="test_device",
201+
baudrate=115200,
201202
vehicle_type="",
202203
reboot_time=5,
203204
n=0,

tests/test_backend_flightcontroller.py

Lines changed: 17 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -17,26 +17,26 @@
1717

1818

1919
def test_add_connection() -> None:
20-
fc = FlightController(reboot_time=7)
20+
fc = FlightController(reboot_time=7, baudrate=115200)
2121
assert fc.add_connection("test_connection") is True
2222
assert fc.add_connection("test_connection") is False
2323
assert fc.add_connection("") is False
2424

2525

2626
def test_discover_connections() -> None:
27-
fc = FlightController(reboot_time=7)
27+
fc = FlightController(reboot_time=7, baudrate=115200)
2828
fc.discover_connections()
2929
assert len(fc.get_connection_tuples()) > 0
3030

3131

3232
def test_connect() -> None:
33-
fc = FlightController(reboot_time=7)
33+
fc = FlightController(reboot_time=7, baudrate=115200)
3434
result = fc.connect(device="test")
3535
assert result == ""
3636

3737

3838
def test_disconnect() -> None:
39-
fc = FlightController(reboot_time=7)
39+
fc = FlightController(reboot_time=7, baudrate=115200)
4040
fc.connect(device="test")
4141
fc.disconnect()
4242
assert fc.master is None
@@ -48,7 +48,7 @@ def test_disconnect() -> None:
4848
side_effect=lambda x: {"param1": Par(1, x), "param2": Par(2, x)},
4949
)
5050
def test_download_params(mock_load_param_file_into_dict, mock_file) -> None:
51-
fc = FlightController(reboot_time=7)
51+
fc = FlightController(reboot_time=7, baudrate=115200)
5252
fc.connect(device="test")
5353
with patch("ardupilot_methodic_configurator.backend_flightcontroller.open", mock_file):
5454
params, _ = fc.download_params()
@@ -63,7 +63,7 @@ def test_download_params(mock_load_param_file_into_dict, mock_file) -> None:
6363
side_effect=lambda x: {"param1": Par(1, x), "param2": Par(2, x)},
6464
)
6565
def test_set_param(mock_load_param_file_into_dict, mock_file) -> None:
66-
fc = FlightController(reboot_time=7)
66+
fc = FlightController(reboot_time=7, baudrate=115200)
6767
fc.connect(device="test")
6868
fc.set_param("TEST_PARAM", 1.0)
6969
with patch("ardupilot_methodic_configurator.backend_flightcontroller.open", mock_file):
@@ -73,22 +73,22 @@ def test_set_param(mock_load_param_file_into_dict, mock_file) -> None:
7373

7474

7575
def test_reset_and_reconnect() -> None:
76-
fc = FlightController(reboot_time=7)
76+
fc = FlightController(reboot_time=7, baudrate=115200)
7777
fc.connect(device="test")
7878
result = fc.reset_and_reconnect()
7979
assert result == ""
8080

8181

8282
def test_upload_file() -> None:
83-
fc = FlightController(reboot_time=7)
83+
fc = FlightController(reboot_time=7, baudrate=115200)
8484
fc.connect(device="test")
8585
result = fc.upload_file("local.txt", "remote.txt")
8686
# Assuming the mock environment always returns False for upload_file
8787
assert result is False
8888

8989

9090
def test_get_connection_tuples() -> None:
91-
fc = FlightController(reboot_time=7)
91+
fc = FlightController(reboot_time=7, baudrate=115200)
9292
fc.add_connection("test_connection")
9393
connections = fc.get_connection_tuples()
9494
assert ("test_connection", "test_connection") in connections
@@ -100,7 +100,7 @@ def test_get_connection_tuples() -> None:
100100
side_effect=lambda x: {"param1": Par(1, x), "param2": Par(2, x)},
101101
)
102102
def test_set_param_and_verify(mock_load_param_file_into_dict, mock_file) -> None:
103-
fc = FlightController(reboot_time=7)
103+
fc = FlightController(reboot_time=7, baudrate=115200)
104104
fc.connect(device="test")
105105
fc.set_param("TEST_PARAM", 1.0)
106106
with patch("ardupilot_methodic_configurator.backend_flightcontroller.open", mock_file):
@@ -111,15 +111,15 @@ def test_set_param_and_verify(mock_load_param_file_into_dict, mock_file) -> None
111111

112112

113113
def test_download_params_via_mavftp() -> None:
114-
fc = FlightController(reboot_time=7)
114+
fc = FlightController(reboot_time=7, baudrate=115200)
115115
fc.connect(device="test")
116116
params, default_params = fc.download_params_via_mavftp()
117117
assert isinstance(params, dict)
118118
assert isinstance(default_params, dict)
119119

120120

121121
def test_auto_detect_serial() -> None:
122-
fc = FlightController(reboot_time=7)
122+
fc = FlightController(reboot_time=7, baudrate=115200)
123123
serial_ports = fc._FlightController__auto_detect_serial() # pylint: disable=protected-access
124124
assert isinstance(serial_ports, list)
125125

@@ -136,34 +136,34 @@ def test_list_network_ports() -> None:
136136

137137

138138
def test_request_banner() -> None:
139-
fc = FlightController(reboot_time=7)
139+
fc = FlightController(reboot_time=7, baudrate=115200)
140140
fc.connect(device="test")
141141
fc._FlightController__request_banner() # pylint: disable=protected-access
142142
# Since we cannot verify in the mock environment, we will just ensure no exceptions are raised
143143

144144

145145
def test_receive_banner_text() -> None:
146-
fc = FlightController(reboot_time=7)
146+
fc = FlightController(reboot_time=7, baudrate=115200)
147147
fc.connect(device="test")
148148
banner_text = fc._FlightController__receive_banner_text() # pylint: disable=protected-access
149149
assert isinstance(banner_text, list)
150150

151151

152152
def test_request_message() -> None:
153-
fc = FlightController(reboot_time=7)
153+
fc = FlightController(reboot_time=7, baudrate=115200)
154154
fc.connect(device="test")
155155
fc._FlightController__request_message(1) # pylint: disable=protected-access
156156
# Since we cannot verify in the mock environment, we will just ensure no exceptions are raised
157157

158158

159159
def test_create_connection_with_retry() -> None:
160-
fc = FlightController(reboot_time=7)
160+
fc = FlightController(reboot_time=7, baudrate=115200)
161161
result = fc._FlightController__create_connection_with_retry(progress_callback=None, retries=1, timeout=1) # pylint: disable=protected-access
162162
assert result == ""
163163

164164

165165
def test_process_autopilot_version() -> None:
166-
fc = FlightController(reboot_time=7)
166+
fc = FlightController(reboot_time=7, baudrate=115200)
167167
fc.connect(device="test")
168168
banner_msgs = ["ChibiOS: 123", "ArduPilot"]
169169
result = fc._FlightController__process_autopilot_version(None, banner_msgs) # pylint: disable=protected-access

0 commit comments

Comments
 (0)