Search, Flight Tracker, and Config Updates

config/search.yaml

@@ -2,7 +2,7 @@
 # Single source of truth for all mission parameters.
 
 # ── Flight ───────────────────────────────────────────────────
-altitude: 4.0              # Search altitude (meters)
+altitude: 4.0              # Search altitude (feet)
 
 # ── ArUco Marker ─────────────────────────────────────────────
 marker:
@@ -13,18 +13,14 @@ marker:
 
 # ── Search Patterns ──────────────────────────────────────────
 spiral:
-  max_legs: 12
+  max_legs: 60
-  initial_leg: 4.0
-  leg_increment: 3.2
 
 lawnmower:
   width: 30.0
   height: 30.0
-  lane_spacing: 3.2
-  lanes: 2
 
 levy:
-  max_steps: 20
+  max_steps: 40
   field_size: 50.0
 
 # ── Geofence ─────────────────────────────────────────────────

config/uav.yaml

@@ -5,3 +5,7 @@ connection:
   sim: "tcp:127.0.0.1:5760"
   real: "/dev/ttyAMA0"
   baud: 57600
+
+speed:
+  min_mph: 0.2
+  max_mph: 1.0

config/ugv.yaml

@@ -11,3 +11,7 @@ position:
 topics:
   cmd_vel: /ugv/cmd_vel
   odometry: /ugv/odometry
+
+speed:
+  min_mph: 0.5
+  max_mph: 3.5

src/control/search.py

@@ -16,13 +16,10 @@ class SearchMode(Enum):
     SPIRAL = "spiral"
     LAWNMOWER = "lawnmower"
     LEVY = "levy"
-
-
 class Search:
-    POSITION_TOLERANCE = 1.0
+    POSITION_TOLERANCE = 0.2
     CHECK_INTERVAL = 0.5
-    MAX_TRAVEL_TIME = 30.0
+    MAX_TRAVEL_TIME = 300.0
-    CAM_FOV_METERS = 4.0
 
     def __init__(self, ctrl: Controller, detector: ObjectDetector,
                  camera=None, mode: str = "spiral", altitude: float = 5.0,
@@ -44,14 +41,17 @@ class Search:
         self.on_target_found = None
         self._dispatched_targets = set()
 
-        self.spiral_max_legs = 12
+        hfov = 1.3962634  # 80 deg horizontal FOV
-        self.spiral_initial_leg = self.CAM_FOV_METERS
+        self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
-        self.spiral_leg_increment = self.CAM_FOV_METERS * 0.8
+
+        self.spiral_max_legs = 40
+        self.spiral_initial_leg = self.cam_fov_meters
+        self.spiral_leg_increment = self.cam_fov_meters * 0.8
 
         self.lawn_width = 30.0
         self.lawn_height = 30.0
-        self.lawn_lane_spacing = self.CAM_FOV_METERS * 0.8
+        self.lawn_lane_spacing = self.cam_fov_meters * 0.8
-        self.lawn_lanes = 2
+        self.lawn_lanes = int(self.lawn_width / max(self.lawn_lane_spacing, 0.1)) + 1
 
         self.levy_max_steps = 20
         self.levy_field_size = 50.0
@@ -159,12 +159,29 @@ class Search:
         return waypoints
 
     def _clip_to_geofence(self, waypoints, polygon):
+        warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
+        safe_polygon = []
+        if warn_dist > 0:
+            cx = sum(p[0] for p in polygon) / len(polygon)
+            cy = sum(p[1] for p in polygon) / len(polygon)
+            for x, y in polygon:
+                dx = x - cx
+                dy = y - cy
+                length = math.sqrt(dx*dx + dy*dy)
+                if length > 0:
+                    shrink = warn_dist / length
+                    safe_polygon.append((x - dx * shrink, y - dy * shrink))
+                else:
+                    safe_polygon.append((x, y))
+        else:
+            safe_polygon = polygon
+
         clipped = []
         for wx, wy in waypoints:
-            if self._point_in_polygon(wx, wy, polygon):
+            if self._point_in_polygon(wx, wy, safe_polygon):
                 clipped.append((wx, wy))
             else:
-                nearest = self._nearest_point_on_polygon(wx, wy, polygon)
+                nearest = self._nearest_point_on_polygon(wx, wy, safe_polygon)
                 if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
                     clipped.append(nearest)
         return clipped
@@ -324,10 +341,13 @@ class Search:
         IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
         HFOV = 1.3962634  # 80° horizontal FOV
 
-        DESCENT_STEP = 1.0
+        self.ctrl.update_state()
-        LAND_ALT = 1.5
+        current_alt = self.ctrl.altitude
+
+        DESCENT_STEP = max(0.5, current_alt * 0.25)
+        LAND_ALT = min(1.0, current_alt - 0.2)
         CENTER_PX = 50
-        MAX_CORRECTIONS = 30
+        MAX_CORRECTIONS = 200
         MAX_LOST = 30
         GAIN = 0.35
 
@@ -365,12 +385,16 @@ class Search:
 
     def center_over_target(self, target_id):
         print(f"\n[SEARCH] ===== CENTERING OVER TARGET {target_id} =====")
+        if hasattr(self, 'uav_min_cms'):
+            print(f"[SEARCH] Reducing speed to minimum ({self.uav_min_cms} cm/s) for centering.")
+            self.ctrl.configure_speed_limits(wpnav_speed=self.uav_min_cms, loit_speed=self.uav_min_cms)
+
         IMG_W, IMG_H = 640, 480
         IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
         HFOV = 1.3962634  # 80° horizontal FOV
 
         CENTER_PX = 30
-        MAX_CORRECTIONS = 30
+        MAX_CORRECTIONS = 200
         MAX_LOST = 30
         GAIN = 0.35
 
@@ -443,14 +467,21 @@ class Search:
             ground_w = 2.0 * alt * math.tan(hfov / 2.0)
             m_per_px = ground_w / img_w
             
-            # Camera pitch-90° inverts image axes relative to NED
+            # Calculate movement in the UAV's body frame
-            correction_y = -err_x * m_per_px * gain
+            correction_forward = -err_y * m_per_px * gain
-            correction_x = -err_y * m_per_px * gain
+            correction_right = err_x * m_per_px * gain
+
+            self.ctrl.update_state()
+            yaw = self.ctrl.current_yaw
+            
+            # Rotate body correction (Forward, Right) into world NED (North, East)
+            correction_n = correction_forward * math.cos(yaw) - correction_right * math.sin(yaw)
+            correction_e = correction_forward * math.sin(yaw) + correction_right * math.cos(yaw)
 
             cur = self.ctrl.get_local_position()
             self.ctrl.move_local_ned(
-                cur['x'] + correction_x,
+                cur['x'] + correction_n,
-                cur['y'] + correction_y,
+                cur['y'] + correction_e,
                 -target_alt)
             sleep(0.5)
 
@@ -460,8 +491,14 @@ class Search:
         return centered
 
     def wait_for_position(self, target_x, target_y, timeout=None):
+        self.ctrl.update_state()
+        pos = self.ctrl.get_local_position()
+        dist_total = distance_2d((pos['x'], pos['y']), (target_x, target_y))
+        
         if timeout is None:
-            timeout = self.MAX_TRAVEL_TIME
+            # Dynamic timeout: 30s base time + 3 seconds per meter
+            timeout = 30.0 + (dist_total * 3.0)
+
         t0 = time.time()
         while time.time() - t0 < timeout and self.running:
             self.ctrl.update_state()

src/control/ugv_controller.py

@@ -22,9 +22,12 @@ YAW_TOL = 0.15
 
 
 class UGVController:
-    def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry"):
+    def __init__(self, cmd_topic="/ugv/cmd_vel", odom_topic="/ugv/odometry",
+                 min_speed=0.5, max_speed=1.0):
         self.node = Node()
         self.pub = self.node.advertise(cmd_topic, Twist)
+        self.min_linear_vel = min_speed
+        self.max_linear_vel = max_speed
         self.position = {'x': 0.0, 'y': 0.0, 'z': 0.0}
         self.yaw = 0.0
         self._lock = threading.Lock()
@@ -59,7 +62,7 @@ class UGVController:
 
     def send_cmd_vel(self, linear_x, angular_z):
         msg = Twist()
-        msg.linear.x = max(-MAX_LINEAR_VEL, min(MAX_LINEAR_VEL, linear_x))
+        msg.linear.x = max(-self.max_linear_vel, min(self.max_linear_vel, linear_x))
         msg.angular.z = max(-MAX_ANGULAR_VEL, min(MAX_ANGULAR_VEL, angular_z))
         self.pub.publish(msg)
 
@@ -117,9 +120,9 @@ class UGVController:
             if abs(yaw_error) > YAW_TOL:
                 angular = max(-MAX_ANGULAR_VEL,
                               min(MAX_ANGULAR_VEL, yaw_error * 2.0))
-                linear = 0.0 if abs(yaw_error) > 0.5 else 0.3
+                linear = 0.0 if abs(yaw_error) > 0.5 else self.min_linear_vel
             else:
-                linear = min(MAX_LINEAR_VEL, dist * 0.5)
+                linear = max(self.min_linear_vel, min(self.max_linear_vel, dist * 0.5))
                 angular = yaw_error * 1.0
 
             self.send_cmd_vel(linear, angular)

src/main.py

@@ -88,7 +88,10 @@ def main():
     search_cfg = load_config('search.yaml')
     ugv_cfg = load_config('ugv.yaml')
 
-    altitude = args.altitude or search_cfg.get('altitude', 5.0)
+    from utils.convert import feet_to_meters, mph_to_ms
+
+    raw_altitude = args.altitude or search_cfg.get('altitude', 4.0)
+    altitude = feet_to_meters(raw_altitude)
     search_mode = args.search
 
     marker_cfg = search_cfg.get('marker', {})
@@ -100,6 +103,14 @@ def main():
     ugv_pos_cfg = ugv_cfg.get('position', {})
     ugv_position = (ugv_pos_cfg.get('x', 0.0), ugv_pos_cfg.get('y', 0.0))
 
+    ugv_speed_cfg = ugv_cfg.get('speed', {})
+    ugv_min_mph = ugv_speed_cfg.get('min_mph', 0.5)
+    ugv_max_mph = ugv_speed_cfg.get('max_mph', 3.5)
+
+    uav_speed_cfg = uav_cfg.get('speed', {})
+    uav_min_mph = uav_speed_cfg.get('min_mph', 1.0)
+    uav_max_mph = uav_speed_cfg.get('max_mph', 15.0)
+
     ugv_topics = ugv_cfg.get('topics', {})
     ugv_cmd_topic = ugv_topics.get('cmd_vel', '/ugv/cmd_vel')
     ugv_odom_topic = ugv_topics.get('odometry', '/ugv/odometry')
@@ -113,7 +124,11 @@ def main():
 
     ctrl = Controller(conn_str)
 
-    ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic)
+    # Apply configuration speed bounds
+    ugv_min_ms = mph_to_ms(ugv_min_mph)
+    ugv_max_ms = mph_to_ms(ugv_max_mph)
+    ugv = UGVController(cmd_topic=ugv_cmd_topic, odom_topic=ugv_odom_topic,
+                        min_speed=ugv_min_ms, max_speed=ugv_max_ms)
 
     detector = ObjectDetector(
         marker_size=marker_size,
@@ -128,9 +143,8 @@ def main():
 
             def detection_overlay(camera_name, frame):
                 detections = detector.detect(frame)
-                if detections:
+                annotated = detector.annotate_frame(frame, detections)
-                    annotated = detector.annotate_frame(frame, detections)
+                camera.frames[camera_name] = annotated
-                    camera.frames[camera_name] = annotated
 
             camera.register_callback("downward", detection_overlay)
             camera.register_callback("gimbal", detection_overlay)
@@ -146,12 +160,15 @@ def main():
     if mode_cfg:
         search.configure(**{f"{search_mode}_{k}": v for k, v in mode_cfg.items()})
 
+    search.uav_min_cms = int(mph_to_ms(uav_min_mph) * 100)
+
     geofence_cfg = search_cfg.get('geofence', {})
     geofence_points = None
     if geofence_cfg.get('enabled', False):
         raw_pts = geofence_cfg.get('points', [])
         if raw_pts:
             geofence_points = [tuple(p) for p in raw_pts]
+            search.geofence_warn_dist = geofence_cfg.get('warning_distance', 3.0)
 
     waypoints = search.plan(start_pos=(0.0, 0.0),
                             geofence_points=geofence_points)
@@ -172,6 +189,12 @@ def main():
     tracker.start()
 
     ctrl.configure_ekf_fast_converge()
+    
+    # Configure AP speeds in cm/s from max_mph
+    uav_max_ms = mph_to_ms(uav_max_mph)
+    uav_max_cms = int(uav_max_ms * 100)
+    ctrl.configure_speed_limits(wpnav_speed=uav_max_cms, loit_speed=uav_max_cms)
+    
     setup_ardupilot(ctrl)
     if recorder:
         recorder.set_phase("ekf_wait")

src/navigation/flight_tracker.py

@@ -20,6 +20,7 @@ class FlightTracker:
     UAV_COLOR = (0, 220, 100)
     UAV_TRAIL_COLOR = (0, 180, 80)
     UGV_COLOR = (60, 60, 255)
+    UGV_TRAIL_COLOR = (40, 40, 200)
     TEXT_COLOR = (200, 200, 200)
     TEXT_DIM_COLOR = (120, 120, 120)
     MARKER_COLOR = (180, 0, 220)
@@ -42,6 +43,7 @@ class FlightTracker:
         self.uav_alt = 0.0
         self.uav_heading = 0.0
         self.trail = deque()
+        self.ugv_trail = deque()
         self.markers_found = {}
 
         self.plan_waypoints = []
@@ -81,6 +83,7 @@ class FlightTracker:
     def update_ugv(self, x, y):
         with self._lock:
             self.ugv_pos = (x, y)
+            self.ugv_trail.append((x, y))
 
     def add_marker(self, marker_id, x, y):
         with self._lock:
@@ -136,6 +139,7 @@ class FlightTracker:
 
         with self._lock:
             trail_copy = list(self.trail)
+            ugv_trail_copy = list(self.ugv_trail)
             uav = self.uav_pos
             uav_alt = self.uav_alt
             uav_hdg = self.uav_heading
@@ -147,7 +151,9 @@ class FlightTracker:
         if plan_copy:
             self._draw_plan(frame, plan_copy, active_wp)
 
-        self._draw_trail(frame, trail_copy)
+        self._draw_trail(frame, trail_copy, self.UAV_TRAIL_COLOR)
+        if self.show_ugv:
+            self._draw_trail(frame, ugv_trail_copy, self.UGV_TRAIL_COLOR)
 
         if self.show_ugv:
             self._draw_ugv(frame, ugv)
@@ -265,12 +271,12 @@ class FlightTracker:
             cv2.putText(frame, str(i), (px + 7, py - 5),
                         cv2.FONT_HERSHEY_SIMPLEX, 0.28, self.PLAN_COLOR, 1)
 
-    def _draw_trail(self, frame, trail):
+    def _draw_trail(self, frame, trail, color):
         if len(trail) < 2:
             return
         pts = [self.world_to_pixel(x, y) for x, y in trail]
         for i in range(1, len(pts)):
-            cv2.line(frame, pts[i - 1], pts[i], self.UAV_TRAIL_COLOR, 2)
+            cv2.line(frame, pts[i - 1], pts[i], color, 2)
 
     def _draw_uav(self, frame, pos, heading):
         px, py = self.world_to_pixel(pos[0], pos[1])
@@ -350,8 +356,6 @@ class FlightTracker:
         else:
             cv2.putText(frame, "None", (x0, y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, self.TEXT_DIM_COLOR, 1)
             y += 16
-        y += 20
-        cv2.putText(frame, f"Trail: {len(trail)} pts", (x0, y), cv2.FONT_HERSHEY_SIMPLEX, 0.35, self.TEXT_DIM_COLOR, 1)
 
     def start(self):
         self.running = True

src/utils/convert.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+"""
+Unit conversions: Customary (US) to Metric.
+"""
+
+def feet_to_meters(feet: float) -> float:
+    return feet * 0.3048
+
+def inches_to_meters(inches: float) -> float:
+    return inches * 0.0254
+
+def miles_to_meters(miles: float) -> float:
+    return miles * 1609.34
+
+def pounds_to_kg(pounds: float) -> float:
+    return pounds * 0.453592
+
+def ounces_to_kg(ounces: float) -> float:
+    return ounces * 0.0283495
+
+def mph_to_ms(mph: float) -> float:
+    return mph * 0.44704

src/vision/object_detector.py

@@ -123,6 +123,13 @@ class ObjectDetector:
 
     def annotate_frame(self, frame, detections):
         annotated = frame.copy()
+        
+        # Draw center crosshair
+        h, w = frame.shape[:2]
+        cx, cy = w // 2, h // 2
+        cv2.line(annotated, (cx - 15, cy), (cx + 15, cy), (0, 255, 0), 2)
+        cv2.line(annotated, (cx, cy - 15), (cx, cy + 15), (0, 255, 0), 2)
+
         if not detections:
             return annotated
         overlay = annotated.copy()