SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Search Landing Offset Fixes

Browse Source
This commit is contained in:
SirBlobby
2026-02-13 15:56:06 -05:00
parent 067c96ed28
commit c7e9f81f55
1 changed files with 125 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

159
src/control/search.py
View File

@@ -122,7 +122,7 @@ class Search:
return new_markers return new_markers
def execute_landing(self, initial_detections): 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 target_det = None
for d in initial_detections: for d in initial_detections:
if d.get("id") in self.land_ids: if d.get("id") in self.land_ids:
@@ -133,28 +133,48 @@ class Search:
print("[SEARCH] Lost landing target, resuming search") print("[SEARCH] Lost landing target, resuming search")
return return
# Phase 1: Fly to marker position using tvec offset print(f"\n[SEARCH] ===== LANDING SEQUENCE =====")
# tvec gives [right, down, forward] from camera in meters 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]) tvec = target_det.get("tvec", [0, 0, 0])
self.ctrl.update_state() self.ctrl.update_state()
pos = self.ctrl.get_local_position() pos = self.ctrl.get_local_position()
# Camera points down: tvec[0]=right=East(+y), tvec[1]=down, tvec[2]=forward=North(+x) # For a downward camera (90° pitch): tvec[0]=right=East, tvec[1]=down=North
marker_x = pos['x'] + tvec[2] # But this is unreliable, so just use it as a rough initial move
marker_y = pos['y'] + tvec[0] 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})") print(f"[SEARCH] Phase 1: Rough approach to ({rough_x:.1f}, {rough_y:.1f})")
self.ctrl.move_local_ned(marker_x, marker_y, -self.altitude) self.ctrl.move_local_ned(rough_x, rough_y, -self.altitude)
self._wait_arrival(marker_x, marker_y, timeout=15.0) self._wait_arrival(rough_x, rough_y, timeout=10.0)
sleep(1.0) # settle
# Phase 2: Hover and refine position using camera feedback # Phase 2: Visual servoing descent
print("[SEARCH] Centering over marker...") current_alt = self.altitude
center_attempts = 0 lost_count = 0
max_attempts = 30 MAX_LOST = 10 # abort if marker lost this many consecutive times
centered_count = 0
while center_attempts < max_attempts and self.running: print(f"[SEARCH] Phase 2: Visual servoing descent from {current_alt:.1f}m")
center_attempts += 1
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() frame = self.get_camera_frame()
if frame is None: if frame is None:
sleep(0.2) sleep(0.2)
@@ -168,46 +188,101 @@ class Search:
break break
if target is None: if target is None:
print(f"\r[SEARCH] Centering: marker not visible ({center_attempts}/{max_attempts}) ", lost_count += 1
print(f"\r[SEARCH] Alt:{current_alt:.1f}m MARKER LOST ({lost_count}/{MAX_LOST}) ",
end='', flush=True) 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) sleep(0.3)
centered_count = 0
continue continue
# Calculate pixel error from image center lost_count = 0
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
print(f"\r[SEARCH] Centering: err=({error_x:.0f},{error_y:.0f})px " # Pixel error from image center
f"dist={target['distance']:.2f}m ({center_attempts}/{max_attempts}) ", 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) end='', flush=True)
# Check if centered enough (within 30px of center) if abs(err_x) < CENTER_PX and abs(err_y) < CENTER_PX:
if abs(error_x) < 30 and abs(error_y) < 30: centered = True
centered_count += 1
if centered_count >= 3:
print(f"\n[SEARCH] Centered over marker!")
break 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)
# 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() self.ctrl.update_state()
cur = self.ctrl.get_local_position() cur = self.ctrl.get_local_position()
new_x = cur['x'] + correction_x * 0.5 # dampen corrections self.ctrl.move_local_ned(cur['x'], cur['y'], -current_alt)
new_y = cur['y'] + correction_y * 0.5 sleep(1.5) # wait for descent
self.ctrl.move_local_ned(new_x, new_y, -self.altitude)
# 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:
sleep(0.3) sleep(0.3)
continue
# Phase 3: Land cx, cy = target["center_px"]
print(f"\n[SEARCH] Landing on target!") err_x = cx - IMG_CX
err_y = cy - IMG_CY
print(f"\r[SEARCH] Final center: err=({err_x:+.0f},{err_y:+.0f})px ",
end='', flush=True)
if abs(err_x) < 25 and abs(err_y) < 25:
print(f"\n[SEARCH] Centered! Landing now.")
break
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)
# Phase 4: Land
print(f"[SEARCH] ===== LANDING =====")
self.ctrl.land() self.ctrl.land()
self.landed = True self.landed = True
self.running = False self.running = False