Add get_current_position()

mavlink/modules/flight_controller.py

@@ -242,6 +242,20 @@ def set_flight_mode(self, mode: str) -> bool:
             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
+
     def get_flight_mode(self) -> "tuple[bool, drone_odometry.FlightMode | None]":
         """
         Gets the current flight mode of the drone.

mavlink/test_flight_controller.py

@@ -69,6 +69,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:

mavlink/test_mission_ended.py

@@ -125,9 +125,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