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

Aruco Tag Landing and Flight Tracker

Browse Source
This commit is contained in:
SirBlobby
2026-02-13 15:36:26 -05:00
parent c91ea920a8
commit 067c96ed28
11 changed files with 929 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

133
src/control/search.py
View File

@@ -37,6 +37,7 @@ class Search:
self.found_markers = {}
self.running = True
self.landed = False
self._landing = False
self.actions = actions or {}
self.land_ids = set(self.actions.get("land", []))
@@ -81,9 +82,9 @@ class Search:
def get_camera_frame(self):
if self.camera is None:
return None
frame = self.camera.frames.get("downward")
frame = self.camera.raw_frames.get("downward")
if frame is None:
frame = self.camera.frames.get("gimbal")
frame = self.camera.raw_frames.get("gimbal")
return frame
def check_for_markers(self):
@@ -112,54 +113,104 @@ class Search:
f"distance:{d['distance']:.2f}m "
f"UAV at ({pos['x']:.1f}, {pos['y']:.1f})")
if marker_id in self.land_ids:
if marker_id in self.land_ids and not self._landing:
print(f"\n[SEARCH] Landing target ID:{marker_id} found! Starting approach.")
self._landing = True
self.execute_landing(detections)
return new_markers
return new_markers
def execute_landing(self, initial_detections):
if self.servoing is None:
self.ctrl.land()
self.landed = True
self.running = False
"""Fly to the marker position, center over it, then land."""
target_det = None
for d in initial_detections:
if d.get("id") in self.land_ids:
target_det = d
break
if target_det is None:
print("[SEARCH] Lost landing target, resuming search")
return
self.servoing.enable()
self.servoing.process_detections(initial_detections)
# Phase 1: Fly to marker position using tvec offset
# tvec gives [right, down, forward] from camera in meters
tvec = target_det.get("tvec", [0, 0, 0])
self.ctrl.update_state()
pos = self.ctrl.get_local_position()
t0 = time.time()
timeout = 60.0
while time.time() - t0 < timeout and self.running:
# 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]
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)
# Phase 2: Hover and refine position using camera feedback
print("[SEARCH] Centering over marker...")
center_attempts = 0
max_attempts = 30
centered_count = 0
while center_attempts < max_attempts and self.running:
center_attempts += 1
frame = self.get_camera_frame()
if frame is None:
sleep(self.CHECK_INTERVAL)
sleep(0.2)
continue
detections = self.detector.detect(frame)
landed = self.servoing.process_detections(detections)
target = None
for d in detections:
if d.get("id") in self.land_ids:
target = d
break
if landed:
print(f"\n[SEARCH] Landed on target!")
self.landed = True
self.running = False
return
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
self.ctrl.update_state()
if self.ctrl.altitude < 0.3:
print(f"\n[SEARCH] Touchdown detected!")
self.landed = True
self.running = False
return
# 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
sleep(0.1)
print(f"\r[SEARCH] Centering: err=({error_x:.0f},{error_y:.0f})px "
f"dist={target['distance']:.2f}m ({center_attempts}/{max_attempts}) ",
end='', flush=True)
if not self.landed:
print(f"\n[SEARCH] Landing approach timed out, landing at current position")
self.ctrl.land()
self.landed = True
self.running = False
# 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)
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)
sleep(0.3)
# Phase 3: Land
print(f"\n[SEARCH] Landing on target!")
self.ctrl.land()
self.landed = True
self.running = False
def wait_for_position(self, target_x, target_y, timeout=None):
if timeout is None:
@@ -186,6 +237,26 @@ class Search:
print()
return False
def _wait_arrival(self, target_x, target_y, timeout=15.0):
"""Wait for position without checking markers (used during landing)."""
t0 = time.time()
while time.time() - t0 < timeout and self.running:
self.ctrl.update_state()
pos = self.ctrl.get_local_position()
dist = distance_2d(
(pos['x'], pos['y']),
(target_x, target_y)
)
elapsed = int(time.time() - t0)
print(f"\r[SEARCH] Approaching marker: {dist:.1f}m ({elapsed}s) ",
end='', flush=True)
if dist < self.POSITION_TOLERANCE:
print()
return True
sleep(self.CHECK_INTERVAL)
print()
return False
def move_to_local(self, x, y):
z = -self.altitude
self.ctrl.move_local_ned(x, y, z)