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

52
src/control/uav_controller.py
View File

@@ -10,8 +10,6 @@ from pymavlink import mavutil
import pymavlink.dialects.v20.ardupilotmega as mavlink
from pymavlink.dialects.v20.ardupilotmega import (
MAVLink_set_position_target_local_ned_message,
MAVLink_set_position_target_global_int_message,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
MAV_FRAME_BODY_OFFSET_NED,
MAV_FRAME_LOCAL_NED,
MAV_CMD_DO_SET_MODE,
@@ -79,17 +77,6 @@ class Controller:
def update_state(self):
self._drain_messages()
def get_position(self) -> dict:
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
mavlink.MAV_CMD_REQUEST_MESSAGE,
0, 33, 0, 0, 0, 0, 0, 0)
pos = self.conn.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=5)
if pos is None:
return {'lat': 0, 'lon': 0}
return {'lat': pos.lat, 'lon': pos.lon}
def get_local_position(self) -> dict:
self.update_state()
return self.position.copy()
@@ -213,17 +200,6 @@ class Controller:
0, 89, 9, 0, 0, 0, 0, 0)
print("[UAV] Landing (LAND mode)")
def land_at(self, lat: int, lon: int):
self.conn.mav.command_long_send(
self.conn.target_system,
self.conn.target_component,
MAV_CMD_NAV_LAND,
0, 0, 0, 0, 0,
lat / 1e7 if abs(lat) > 1e6 else lat,
lon / 1e7 if abs(lon) > 1e6 else lon,
0)
print(f"[UAV] Landing at ({lat}, {lon})")
def move_pos_rel(self, x: float, y: float, z: float):
move_msg = MAVLink_set_position_target_local_ned_message(
int(perf_counter() * 1000),
@@ -264,16 +240,6 @@ class Controller:
0, 0, 0, 0, 0, 0, 0, 0)
self.conn.mav.send(move_msg)
def go_to(self, lat: int, lon: int, alt: float, yaw: float = 0):
move_msg = MAVLink_set_position_target_global_int_message(
int(perf_counter() * 1000),
self.conn.target_system,
self.conn.target_component,
MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
504, lat, lon, alt,
0, 0, 0, 0, 0, 0, yaw, 0)
self.conn.mav.send(move_msg)
def distance(self, p1: Tuple, p2: Tuple) -> float:
return float(np.linalg.norm(np.asarray(p1) - np.asarray(p2)))
@@ -290,24 +256,6 @@ class Controller:
sleep(0.2)
return False
def wait_for_gps(self, timeout: float = 120.0):
print("[UAV] Waiting for GPS lock & EKF ...")
t0 = time.time()
while time.time() - t0 < timeout:
self._drain_messages()
msg = self.conn.recv_match(type='GPS_RAW_INT', blocking=True, timeout=2)
if msg and msg.fix_type >= 3:
print(f"\n[UAV] GPS fix: {msg.fix_type} sats: {msg.satellites_visible}")
print("[UAV] Waiting for EKF to settle ...")
# Wait briefly; let the arming loop handle the rest
for _ in range(2):
self._drain_messages()
sleep(1)
return True
elapsed = int(time.time() - t0)
print(f"\r[UAV] Waiting for GPS ... {elapsed}s ", end='', flush=True)
return False
def configure_speed_limits(self,
wpnav_speed=DEFAULT_WPNAV_SPEED,
wpnav_accel=DEFAULT_WPNAV_ACCEL,

4
src/main.py
View File

@@ -174,8 +174,8 @@ def main():
ctrl.configure_ekf_fast_converge()
setup_ardupilot(ctrl)
if recorder:
recorder.set_phase("gps_wait")
ctrl.wait_for_gps()
recorder.set_phase("ekf_wait")
ctrl.wait_for_ekf()
if not ctrl.arm():
print("[UAV] Cannot arm - aborting")

18
src/navigation/flight_tracker.py
View File

@@ -268,23 +268,9 @@ class FlightTracker:
def _draw_trail(self, frame, trail):
if len(trail) < 2:
return
MIN_ALPHA = 0.3
FADE_POINTS = 200
pts = [self.world_to_pixel(x, y) for x, y in trail]
n = len(pts)
fade_start = max(0, n - FADE_POINTS)
for i in range(1, n):
if i <= fade_start:
alpha = MIN_ALPHA
else:
progress = (i - fade_start) / FADE_POINTS
alpha = MIN_ALPHA + (1.0 - MIN_ALPHA) * progress
color = (int(self.UAV_TRAIL_COLOR[0] * alpha),
int(self.UAV_TRAIL_COLOR[1] * alpha),
int(self.UAV_TRAIL_COLOR[2] * alpha))
cv2.line(frame, pts[i - 1], pts[i], color, 2)
for i in range(1, len(pts)):
cv2.line(frame, pts[i - 1], pts[i], self.UAV_TRAIL_COLOR, 2)
def _draw_uav(self, frame, pos, heading):
px, py = self.world_to_pixel(pos[0], pos[1])