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

Search and Flight Tracker Update

Browse Source
This commit is contained in:
SirBlobby
2026-02-20 16:11:10 -05:00
parent e2f805f3f3
commit e20c7e916d
4 changed files with 43 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/control/search.py
View File

@@ -287,11 +287,14 @@ class Search:
if marker_id in self.target_ids and marker_id not in self._dispatched_targets:
self._dispatched_targets.add(marker_id)
pos = self.found_markers[marker_id]['uav_position']
print(f"\n[SEARCH] Target ID:{marker_id} found! "
f"Dispatching UGV to ({pos['x']:.1f}, {pos['y']:.1f})")
self.running = False
final_pos = self.center_over_target(marker_id)
print(f"\n[SEARCH] Target ID:{marker_id} centered! "
f"Dispatching UGV to ({final_pos['x']:.1f}, {final_pos['y']:.1f})")
if self.on_target_found:
self.on_target_found(marker_id, pos['x'], pos['y'])
self.on_target_found(marker_id, final_pos['x'], final_pos['y'])
if marker_id in self.land_ids and not self._landing and self.landing_enabled:
print(f"\n[SEARCH] Landing target ID:{marker_id} found! "
@@ -360,8 +363,38 @@ class Search:
self.landed = True
self.running = False
def center_over_target(self, target_id):
print(f"\n[SEARCH] ===== CENTERING OVER TARGET {target_id} =====")
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_LOST = 30
GAIN = 0.35
self.ctrl.update_state()
current_alt = self.ctrl.altitude
centered = self._center_over_marker(
current_alt, IMG_W, IMG_H, IMG_CX, IMG_CY, HFOV,
CENTER_PX, MAX_CORRECTIONS, MAX_LOST, GAIN, allowed_ids=[target_id])
if not centered:
print(f"\n[SEARCH] Failed to fully center, using best estimate.")
else:
print(f"\n[SEARCH] Successfully centered over target {target_id}.")
self.ctrl.update_state()
pos = self.ctrl.get_local_position()
return pos
def _center_over_marker(self, target_alt, img_w, img_h, img_cx, img_cy,
hfov, center_px, max_corrections, max_lost, gain):
hfov, center_px, max_corrections, max_lost, gain, allowed_ids=None):
if allowed_ids is None:
allowed_ids = self.land_ids
lost_count = 0
centered = False
@@ -374,7 +407,7 @@ class Search:
detections = self.detector.detect(frame)
target = None
for d in detections:
if d.get("id") in self.land_ids:
if d.get("id") in allowed_ids:
target = d
break