From 6556e444164c493129114f44160ba5d75ad74e2b Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 20 Aug 2024 11:19:58 -0600 Subject: [PATCH 1/6] Added timeout to upload_commands and download_commands functions. --- mavlink/modules/flight_controller.py | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index 2ca24ed..96c4072 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -114,7 +114,7 @@ def get_home_location( return True, location - def upload_commands(self, commands: "list[dronekit.Command]") -> bool: + def upload_commands(self, commands: "list[dronekit.Command]", timeout: float = 10.0) -> bool: """ Writes a mission to the drone from a list of commands (will overwrite any previous missions). @@ -132,7 +132,7 @@ def upload_commands(self, commands: "list[dronekit.Command]") -> bool: try: command_sequence = self.drone.commands command_sequence.download() - command_sequence.wait_ready() + command_sequence.wait_ready(timeout) command_sequence.clear() for command in commands: command_sequence.add(command) @@ -148,7 +148,7 @@ def upload_commands(self, commands: "list[dronekit.Command]") -> bool: return True - def upload_land_command(self, latitude: float, longitude: float) -> bool: + def upload_land_command(self, latitude: float, longitude: float, timeout: float) -> bool: """ Given a target latitude and longitude, overwrite the drone's current mission with a corresponding land command. @@ -180,7 +180,7 @@ def upload_land_command(self, latitude: float, longitude: float) -> bool: 0, ) - return self.upload_commands([landing_command]) + return self.upload_commands([landing_command], timeout) def is_drone_destination_final_waypoint(self) -> "tuple[bool, bool | None]": """ @@ -255,7 +255,7 @@ def get_flight_mode(self) -> "tuple[bool, drone_odometry.FlightMode | None]": return True, drone_odometry.FlightMode.MOVING return True, drone_odometry.FlightMode.MANUAL - def download_commands(self) -> "tuple[bool, list[dronekit.Command]]": + def download_commands(self, timeout: float = 10.0) -> "tuple[bool, list[dronekit.Command]]": """ Downloads the current list of commands from the drone. @@ -268,7 +268,7 @@ def download_commands(self) -> "tuple[bool, list[dronekit.Command]]": try: command_sequence = self.drone.commands command_sequence.download() - command_sequence.wait_ready() + command_sequence.wait_ready(timeout) commands = list(command_sequence) return True, commands except dronekit.TimeoutError: @@ -278,7 +278,7 @@ def download_commands(self) -> "tuple[bool, list[dronekit.Command]]": print("ERROR: Connection with drone reset. Unable to download commands.") return False, [] - def get_next_waypoint(self) -> "tuple[bool, drone_odometry.DronePosition | None]": + def get_next_waypoint(self, timeout: float) -> "tuple[bool, drone_odometry.DronePosition | None]": """ Gets the next waypoint. @@ -288,7 +288,7 @@ def get_next_waypoint(self) -> "tuple[bool, drone_odometry.DronePosition | None] A tuple where the first element is a boolean indicating success or failure, and the second element is the next waypoint currently held by the drone. """ - result, commands = self.download_commands() + result, commands = self.download_commands(timeout) if not result: return False, None @@ -302,12 +302,12 @@ def get_next_waypoint(self) -> "tuple[bool, drone_odometry.DronePosition | None] return False, None def insert_waypoint( - self, index: int, latitude: float, longitude: float, altitude: float + self, index: int, latitude: float, longitude: float, altitude: float, timeout: float ) -> bool: """ Insert a waypoint into the current list of commands at a certain index and reupload the list to the drone. """ - result, commands = self.download_commands() + result, commands = self.download_commands(timeout) if not result: return False @@ -330,4 +330,4 @@ def insert_waypoint( commands.insert(index, new_waypoint) - return self.upload_commands(commands) + return self.upload_commands(commands, timeout) From 907eb1f5a4e3b8dbb1d9912b88b5934358153ae7 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 20 Aug 2024 11:25:55 -0600 Subject: [PATCH 2/6] Updated test_flight_controller --- mavlink/test_flight_controller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mavlink/test_flight_controller.py b/mavlink/test_flight_controller.py index 61320b0..646117d 100644 --- a/mavlink/test_flight_controller.py +++ b/mavlink/test_flight_controller.py @@ -48,7 +48,7 @@ def main() -> int: time.sleep(DELAY_TIME) # Download and print commands - success, commands = controller.download_commands() + success, commands = controller.download_commands(TIMEOUT) if success: print("Downloaded commands:") for command in commands: @@ -56,7 +56,7 @@ def main() -> int: else: print("Failed to download commands.") - result, next_waypoint = controller.get_next_waypoint() + result, next_waypoint = controller.get_next_waypoint(TIMEOUT) if result: print("next waypoint lat: " + str(next_waypoint.latitude)) print("next waypoint lon: " + str(next_waypoint.longitude)) @@ -70,7 +70,7 @@ def main() -> int: return -1 # Create and add land command - result = controller.upload_land_command(home.latitude, home.longitude) + result = controller.upload_land_command(home.latitude, home.longitude, TIMEOUT) if not result: print("Could not upload land command.") return -1 From 422a749258570ebc473777261026fa67374db7c5 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 20 Aug 2024 11:30:37 -0600 Subject: [PATCH 3/6] Fixed linter issue --- mavlink/modules/flight_controller.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index 96c4072..399b68a 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -278,7 +278,9 @@ def download_commands(self, timeout: float = 10.0) -> "tuple[bool, list[dronekit print("ERROR: Connection with drone reset. Unable to download commands.") return False, [] - def get_next_waypoint(self, timeout: float) -> "tuple[bool, drone_odometry.DronePosition | None]": + def get_next_waypoint( + self, timeout: float + ) -> "tuple[bool, drone_odometry.DronePosition | None]": """ Gets the next waypoint. From 98b11995164c43fbdc6e11f6835b72b84fb5c1cb Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Tue, 20 Aug 2024 11:42:54 -0600 Subject: [PATCH 4/6] Updated docstrings --- mavlink/modules/flight_controller.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index 399b68a..4eb68f3 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -121,6 +121,7 @@ def upload_commands(self, commands: "list[dronekit.Command]", timeout: float = 1 Parameters ---------- commands: List of commands. + timeout: Seconds (default of 10.0 seconds). Returns ------- @@ -157,6 +158,7 @@ def upload_land_command(self, latitude: float, longitude: float, timeout: float) ---------- latitude: Decimal degrees. longitude: Decimal degrees. + timeout: Seconds. Returns ------- @@ -259,6 +261,10 @@ def download_commands(self, timeout: float = 10.0) -> "tuple[bool, list[dronekit """ Downloads the current list of commands from the drone. + Parameters + ---------- + timeout: Seconds (default of 10.0 seconds). + Returns ------- tuple[bool, list[dronekit.Command]] @@ -284,6 +290,10 @@ def get_next_waypoint( """ Gets the next waypoint. + Parameters + ---------- + timeout: Seconds. + Returns ------- tuple[bool, drone_odometry.DronePosition | None] From 68a7c386dfed6ff22c9e0490503999595fe112e4 Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sat, 24 Aug 2024 18:56:29 -0600 Subject: [PATCH 5/6] Changed default value to 30.0 --- mavlink/modules/flight_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index 4eb68f3..a1274f5 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -114,7 +114,7 @@ def get_home_location( return True, location - def upload_commands(self, commands: "list[dronekit.Command]", timeout: float = 10.0) -> bool: + def upload_commands(self, commands: "list[dronekit.Command]", timeout: float = 30.0) -> bool: """ Writes a mission to the drone from a list of commands (will overwrite any previous missions). @@ -257,7 +257,7 @@ def get_flight_mode(self) -> "tuple[bool, drone_odometry.FlightMode | None]": return True, drone_odometry.FlightMode.MOVING return True, drone_odometry.FlightMode.MANUAL - def download_commands(self, timeout: float = 10.0) -> "tuple[bool, list[dronekit.Command]]": + def download_commands(self, timeout: float = 30.0) -> "tuple[bool, list[dronekit.Command]]": """ Downloads the current list of commands from the drone. From 63367b11f8ba67c05cdc7d4de7777599ab9f415e Mon Sep 17 00:00:00 2001 From: HermanG05 Date: Sat, 24 Aug 2024 18:57:03 -0600 Subject: [PATCH 6/6] Changed default value to 30 --- mavlink/modules/flight_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index a1274f5..c1cad14 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -121,7 +121,7 @@ def upload_commands(self, commands: "list[dronekit.Command]", timeout: float = 3 Parameters ---------- commands: List of commands. - timeout: Seconds (default of 10.0 seconds). + timeout: Seconds (default of 30.0 seconds). Returns ------- @@ -263,7 +263,7 @@ def download_commands(self, timeout: float = 30.0) -> "tuple[bool, list[dronekit Parameters ---------- - timeout: Seconds (default of 10.0 seconds). + timeout: Seconds (default of 30.0 seconds). Returns -------