From 0d1ece5d7738c7b0a38558382a8d72bb26b3a066 Mon Sep 17 00:00:00 2001 From: trotyl Date: Tue, 24 Sep 2024 19:43:55 -0400 Subject: [PATCH 1/3] add get current position method --- mavlink/modules/flight_controller.py | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index b64b87b..699e914 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -235,3 +235,17 @@ def set_flight_mode(self, mode: str) -> bool: print("ERROR: an unsupported flight mode is set by dronekit.VehicleMode()") return False return True + + def get_current_position(self) -> drone_odometry.DronePosition: + """ + Retrieves the current position of the drone. + """ + location = self.drone.location.global_frame + result, position = drone_odometry.DronePosition.create( + location.lat, + location.lon, + location.alt, + ) + if not result or position is None: + raise RuntimeError("Failed to get current position") + return position \ No newline at end of file From 7bea4451a1c58c2f0e3de378f5ad9c8a0d29cbb2 Mon Sep 17 00:00:00 2001 From: trotyl Date: Tue, 24 Sep 2024 20:15:53 -0400 Subject: [PATCH 2/3] update flight controller test --- mavlink/test_flight_controller.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mavlink/test_flight_controller.py b/mavlink/test_flight_controller.py index f032fb4..bf2aee9 100644 --- a/mavlink/test_flight_controller.py +++ b/mavlink/test_flight_controller.py @@ -52,6 +52,18 @@ def main() -> int: print("Failed to get home location") return -1 + # Testing get_current_position() + print("\nTesting get_current_position():") + for _ in range(5): + result, current_position = controller.get_current_position() + if result and current_position is not None: + print(f"Current Latitude: {current_position.latitude}") + print(f"Current Longitude: {current_position.longitude}") + print(f"Current Altitude: {current_position.altitude}") + else: + print("Failed to get current position") + time.sleep(DELAY_TIME) + # Create and add land command result = controller.upload_land_command(home.latitude, home.longitude) if not result: From 451e03dd81480a46228b2a4dec36a3c336ca095b Mon Sep 17 00:00:00 2001 From: trotyl Date: Tue, 24 Sep 2024 20:18:43 -0400 Subject: [PATCH 3/3] format files --- mavlink/modules/flight_controller.py | 4 ++-- mavlink/test_mission_ended.py | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/mavlink/modules/flight_controller.py b/mavlink/modules/flight_controller.py index 699e914..97b92fa 100644 --- a/mavlink/modules/flight_controller.py +++ b/mavlink/modules/flight_controller.py @@ -235,7 +235,7 @@ def set_flight_mode(self, mode: str) -> bool: print("ERROR: an unsupported flight mode is set by dronekit.VehicleMode()") return False return True - + def get_current_position(self) -> drone_odometry.DronePosition: """ Retrieves the current position of the drone. @@ -248,4 +248,4 @@ def get_current_position(self) -> drone_odometry.DronePosition: ) if not result or position is None: raise RuntimeError("Failed to get current position") - return position \ No newline at end of file + return position diff --git a/mavlink/test_mission_ended.py b/mavlink/test_mission_ended.py index 898fffc..9589553 100644 --- a/mavlink/test_mission_ended.py +++ b/mavlink/test_mission_ended.py @@ -124,9 +124,10 @@ def main() -> int: return -1 while True: - result, is_drone_destination_final_waypoint = ( - controller.is_drone_destination_final_waypoint() - ) + ( + result, + is_drone_destination_final_waypoint, + ) = controller.is_drone_destination_final_waypoint() if not result: print("Failed to get if the drone's destination is the final waypoint.") return -1