add get_current_position method to flight_controller

mavlink/modules/flight_controller.py

@@ -332,3 +332,21 @@ def insert_waypoint(
         commands.insert(index, new_waypoint)
 
         return self.upload_commands(commands)
+
+    def get_current_position(self) -> "tuple[bool, drone_odometry.DronePosition | None]":
+        """
+        Retrieves the current position of the drone.
+
+        Returns:
+            tuple[bool, DronePosition | None]: A tuple containing a boolean indicating success,
+            and the current DronePosition or None.
+        """
+        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:
+            return False, None
+        return True, position

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: