From c7e9f81f55f7b9de4b9a5fff8da98879abf1ea30 Mon Sep 17 00:00:00 2001 From: SirBlobby Date: Fri, 13 Feb 2026 15:56:06 -0500 Subject: [PATCH] Search Landing Offset Fixes --- src/control/search.py | 175 ++++++++++++++++++++++++++++++------------ 1 file changed, 125 insertions(+), 50 deletions(-) diff --git a/src/control/search.py b/src/control/search.py index 8244154..3281acf 100644 --- a/src/control/search.py +++ b/src/control/search.py @@ -122,7 +122,7 @@ class Search: return new_markers def execute_landing(self, initial_detections): - """Fly to the marker position, center over it, then land.""" + """Visual-servoing descent: keep the ArUco tag centered while descending.""" target_det = None for d in initial_detections: if d.get("id") in self.land_ids: @@ -133,81 +133,156 @@ class Search: print("[SEARCH] Lost landing target, resuming search") return - # Phase 1: Fly to marker position using tvec offset - # tvec gives [right, down, forward] from camera in meters + print(f"\n[SEARCH] ===== LANDING SEQUENCE =====") + print(f"[SEARCH] Target marker ID:{target_det['id']} " + f"distance:{target_det['distance']:.2f}m") + + # Camera parameters + IMG_W, IMG_H = 640, 480 + IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2 + HFOV = 1.3962634 # radians (~80 degrees) + + # Landing parameters + DESCENT_STEP = 1.0 # descend 1m per step + LAND_ALT = 1.5 # switch to blind land at this altitude + CENTER_PX = 40 # centered if within this many pixels + MAX_CORRECTIONS = 15 # max correction iterations per altitude step + GAIN = 0.4 # damping factor for corrections + + # Phase 1: Initial approach — fly toward marker using tvec as rough guide tvec = target_det.get("tvec", [0, 0, 0]) self.ctrl.update_state() pos = self.ctrl.get_local_position() - # Camera points down: tvec[0]=right=East(+y), tvec[1]=down, tvec[2]=forward=North(+x) - marker_x = pos['x'] + tvec[2] - marker_y = pos['y'] + tvec[0] + # For a downward camera (90° pitch): tvec[0]=right=East, tvec[1]=down=North + # But this is unreliable, so just use it as a rough initial move + rough_x = pos['x'] + tvec[1] + rough_y = pos['y'] + tvec[0] - print(f"[SEARCH] Flying to marker at NED ({marker_x:.1f}, {marker_y:.1f})") - self.ctrl.move_local_ned(marker_x, marker_y, -self.altitude) - self._wait_arrival(marker_x, marker_y, timeout=15.0) + print(f"[SEARCH] Phase 1: Rough approach to ({rough_x:.1f}, {rough_y:.1f})") + self.ctrl.move_local_ned(rough_x, rough_y, -self.altitude) + self._wait_arrival(rough_x, rough_y, timeout=10.0) + sleep(1.0) # settle - # Phase 2: Hover and refine position using camera feedback - print("[SEARCH] Centering over marker...") - center_attempts = 0 - max_attempts = 30 - centered_count = 0 + # Phase 2: Visual servoing descent + current_alt = self.altitude + lost_count = 0 + MAX_LOST = 10 # abort if marker lost this many consecutive times - while center_attempts < max_attempts and self.running: - center_attempts += 1 + print(f"[SEARCH] Phase 2: Visual servoing descent from {current_alt:.1f}m") + + while current_alt > LAND_ALT and self.running: + # Center over marker at current altitude + centered = False + for attempt in range(MAX_CORRECTIONS): + frame = self.get_camera_frame() + if frame is None: + sleep(0.2) + continue + + detections = self.detector.detect(frame) + target = None + for d in detections: + if d.get("id") in self.land_ids: + target = d + break + + if target is None: + lost_count += 1 + print(f"\r[SEARCH] Alt:{current_alt:.1f}m MARKER LOST ({lost_count}/{MAX_LOST}) ", + end='', flush=True) + if lost_count >= MAX_LOST: + print(f"\n[SEARCH] Marker lost too many times, aborting landing") + self._landing = False + return + sleep(0.3) + continue + + lost_count = 0 + + # Pixel error from image center + cx, cy = target["center_px"] + err_x = cx - IMG_CX # positive = marker is right of center + err_y = cy - IMG_CY # positive = marker is below center + + print(f"\r[SEARCH] Alt:{current_alt:.1f}m err=({err_x:+.0f},{err_y:+.0f})px " + f"dist:{target['distance']:.2f}m ({attempt+1}/{MAX_CORRECTIONS}) ", + end='', flush=True) + + if abs(err_x) < CENTER_PX and abs(err_y) < CENTER_PX: + centered = True + break + + # Convert pixel error to meters using FOV and altitude + # At current altitude, the ground plane width visible is: + # ground_width = 2 * alt * tan(HFOV/2) + self.ctrl.update_state() + alt = max(self.ctrl.altitude, 0.5) + ground_w = 2.0 * alt * math.tan(HFOV / 2.0) + m_per_px = ground_w / IMG_W + + # Apply correction (pixel right = East = +y, pixel down = North = +x) + correction_y = err_x * m_per_px * GAIN # East + correction_x = err_y * m_per_px * GAIN # North + + cur = self.ctrl.get_local_position() + new_x = cur['x'] + correction_x + new_y = cur['y'] + correction_y + self.ctrl.move_local_ned(new_x, new_y, -current_alt) + sleep(0.4) + + if not centered: + print(f"\n[SEARCH] Could not fully center at {current_alt:.1f}m, continuing descent") + + # Descend one step + current_alt = max(current_alt - DESCENT_STEP, LAND_ALT) + print(f"\n[SEARCH] Descending to {current_alt:.1f}m") + self.ctrl.update_state() + cur = self.ctrl.get_local_position() + self.ctrl.move_local_ned(cur['x'], cur['y'], -current_alt) + sleep(1.5) # wait for descent + + # Phase 3: Final centering at low altitude + print(f"[SEARCH] Phase 3: Final centering at {LAND_ALT:.1f}m") + for attempt in range(MAX_CORRECTIONS): frame = self.get_camera_frame() if frame is None: sleep(0.2) continue - detections = self.detector.detect(frame) target = None for d in detections: if d.get("id") in self.land_ids: target = d break - if target is None: - print(f"\r[SEARCH] Centering: marker not visible ({center_attempts}/{max_attempts}) ", - end='', flush=True) sleep(0.3) - centered_count = 0 continue - # Calculate pixel error from image center - center_px = target["center_px"] - img_cx, img_cy = 320, 240 - error_x = center_px[0] - img_cx # positive = marker is right - error_y = center_px[1] - img_cy # positive = marker is below + cx, cy = target["center_px"] + err_x = cx - IMG_CX + err_y = cy - IMG_CY - print(f"\r[SEARCH] Centering: err=({error_x:.0f},{error_y:.0f})px " - f"dist={target['distance']:.2f}m ({center_attempts}/{max_attempts}) ", + print(f"\r[SEARCH] Final center: err=({err_x:+.0f},{err_y:+.0f})px ", end='', flush=True) - # Check if centered enough (within 30px of center) - if abs(error_x) < 30 and abs(error_y) < 30: - centered_count += 1 - if centered_count >= 3: - print(f"\n[SEARCH] Centered over marker!") - break - else: - centered_count = 0 - # Send small position corrections - # Convert pixel error to meters (rough: at 5m alt, 640px ~ 8m FOV) - meters_per_px = (self.altitude * 0.0025) - correction_y = error_x * meters_per_px # pixel right -> NED east (+y) - correction_x = error_y * meters_per_px # pixel down -> NED north (+x) + if abs(err_x) < 25 and abs(err_y) < 25: + print(f"\n[SEARCH] Centered! Landing now.") + break - self.ctrl.update_state() - cur = self.ctrl.get_local_position() - new_x = cur['x'] + correction_x * 0.5 # dampen corrections - new_y = cur['y'] + correction_y * 0.5 - self.ctrl.move_local_ned(new_x, new_y, -self.altitude) + self.ctrl.update_state() + alt = max(self.ctrl.altitude, 0.5) + ground_w = 2.0 * alt * math.tan(HFOV / 2.0) + m_per_px = ground_w / IMG_W + correction_y = err_x * m_per_px * 0.3 + correction_x = err_y * m_per_px * 0.3 + cur = self.ctrl.get_local_position() + self.ctrl.move_local_ned(cur['x'] + correction_x, + cur['y'] + correction_y, -LAND_ALT) + sleep(0.5) - sleep(0.3) - - # Phase 3: Land - print(f"\n[SEARCH] Landing on target!") + # Phase 4: Land + print(f"[SEARCH] ===== LANDING =====") self.ctrl.land() self.landed = True self.running = False