diff --git a/src/control/search.py b/src/control/search.py index 750ec2c..2853364 100644 --- a/src/control/search.py +++ b/src/control/search.py @@ -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 diff --git a/src/control/uav_controller.py b/src/control/uav_controller.py index 3c8f4b1..028ada4 100644 --- a/src/control/uav_controller.py +++ b/src/control/uav_controller.py @@ -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, diff --git a/src/main.py b/src/main.py index 98fd485..13a3f30 100644 --- a/src/main.py +++ b/src/main.py @@ -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") diff --git a/src/navigation/flight_tracker.py b/src/navigation/flight_tracker.py index 22f57e8..68c98ec 100644 --- a/src/navigation/flight_tracker.py +++ b/src/navigation/flight_tracker.py @@ -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])