From 3eb828195195d4674db9ad4cbed74e2eb8cbeabe Mon Sep 17 00:00:00 2001
From: stash <pomichterstash@gmail.com>
Date: Thu, 25 Sep 2025 00:21:22 -0700
Subject: [PATCH] Added is_flying_to_target agent skill and fly_to now return
 string for agent feeback

---
 dimos/robot/drone/connection_module.py  | 19 +++++++++++++++----
 dimos/robot/drone/mavlink_connection.py | 25 +++++++++++++++++--------
 2 files changed, 32 insertions(+), 12 deletions(-)

diff --git a/dimos/robot/drone/connection_module.py b/dimos/robot/drone/connection_module.py
index 33b5562ec5..79dde2e09d 100644
--- a/dimos/robot/drone/connection_module.py
+++ b/dimos/robot/drone/connection_module.py
@@ -308,8 +308,19 @@ def move_twist(self, twist, duration: float = 0.0, lock_altitude: bool = True) -
         return False
 
     @skill()
-    def fly_to(self, lat: float, lon: float, alt: float) -> bool:
-        """Fly drone to GPS coordinates.
+    def is_flying_to_target(self) -> bool:
+        """Check if drone is currently flying to a GPS target.
+
+        Returns:
+            True if flying to target, False otherwise
+        """
+        if self.connection and hasattr(self.connection, "is_flying_to_target"):
+            return self.connection.is_flying_to_target
+        return False
+
+    @skill()
+    def fly_to(self, lat: float, lon: float, alt: float) -> str:
+        """Fly drone to GPS coordinates (blocking operation).
 
         Args:
             lat: Latitude in degrees
@@ -317,11 +328,11 @@ def fly_to(self, lat: float, lon: float, alt: float) -> bool:
             alt: Altitude in meters (relative to home)
 
         Returns:
-            True if command sent successfully
+            String message indicating success or failure reason
         """
         if self.connection:
             return self.connection.fly_to(lat, lon, alt)
-        return False
+        return "Failed: No connection to drone"
 
     @rpc
     def stop(self):
diff --git a/dimos/robot/drone/mavlink_connection.py b/dimos/robot/drone/mavlink_connection.py
index 4186d88d7d..6f021b5591 100644
--- a/dimos/robot/drone/mavlink_connection.py
+++ b/dimos/robot/drone/mavlink_connection.py
@@ -704,7 +704,7 @@ def land(self) -> bool:
         logger.warning("Land timeout")
         return self.set_mode("LAND")
 
-    def fly_to(self, lat: float, lon: float, alt: float) -> bool:
+    def fly_to(self, lat: float, lon: float, alt: float) -> str:
         """Fly to GPS coordinates - sends commands continuously until reaching target.
 
         Args:
@@ -713,15 +713,19 @@ def fly_to(self, lat: float, lon: float, alt: float) -> bool:
             alt: Altitude in meters (relative to home)
 
         Returns:
-            True if target reached, False if timeout or error
+            String message indicating success or failure reason
         """
         if not self.connected:
-            return False
+            return "Failed: Not connected to drone"
 
         # Check if already flying to a target
         if self.flying_to_target:
-            logger.warning("Already flying to target, ignoring new fly_to command")
-            return False
+            logger.warning(
+                "Already flying to target, ignoring new fly_to command. Wait until completed to send new fly_to command."
+            )
+            return (
+                "Already flying to target - wait for completion before sending new fly_to command"
+            )
 
         self.flying_to_target = True
 
@@ -729,7 +733,7 @@ def fly_to(self, lat: float, lon: float, alt: float) -> bool:
         if not self.set_mode("GUIDED"):
             logger.error("Failed to set GUIDED mode for GPS navigation")
             self.flying_to_target = False
-            return False
+            return "Failed: Could not set GUIDED mode for GPS navigation"
 
         logger.info(f"Flying to GPS: lat={lat:.7f}, lon={lon:.7f}, alt={alt:.1f}m")
 
@@ -820,7 +824,7 @@ def fly_to(self, lat: float, lon: float, alt: float) -> bool:
                         self.set_mode("STABILIZE")
                         logger.info("Returned to STABILIZE mode for manual control")
                         self.flying_to_target = False
-                        return True  # Successfully reached target
+                        return f"Success: Reached target location (lat={lat:.7f}, lon={lon:.7f}, alt={alt:.1f}m)"
 
                     # Only send velocity commands if we're far enough
                     if distance > 0.1:
@@ -930,7 +934,7 @@ def fly_to(self, lat: float, lon: float, alt: float) -> bool:
                 self.set_mode("STABILIZE")
                 logger.info("Returned to STABILIZE mode for manual control")
 
-        return False  # Timeout - did not reach target
+        return "Failed: Timeout - did not reach target within 120 seconds"
 
     def set_mode(self, mode: str) -> bool:
         """Set flight mode."""
@@ -1007,6 +1011,11 @@ def disconnect(self):
         self.connected = False
         logger.info("Disconnected")
 
+    @property
+    def is_flying_to_target(self) -> bool:
+        """Check if drone is currently flying to a GPS target."""
+        return self.flying_to_target
+
     def get_video_stream(self, fps: int = 30):
         """Get video stream (to be implemented with GStreamer)."""
         # Will be implemented in camera module