Search, Flight Tracker, and Config Updates
This commit is contained in:
@@ -16,13 +16,10 @@ class SearchMode(Enum):
|
||||
SPIRAL = "spiral"
|
||||
LAWNMOWER = "lawnmower"
|
||||
LEVY = "levy"
|
||||
|
||||
|
||||
class Search:
|
||||
POSITION_TOLERANCE = 1.0
|
||||
POSITION_TOLERANCE = 0.2
|
||||
CHECK_INTERVAL = 0.5
|
||||
MAX_TRAVEL_TIME = 30.0
|
||||
CAM_FOV_METERS = 4.0
|
||||
MAX_TRAVEL_TIME = 300.0
|
||||
|
||||
def __init__(self, ctrl: Controller, detector: ObjectDetector,
|
||||
camera=None, mode: str = "spiral", altitude: float = 5.0,
|
||||
@@ -44,14 +41,17 @@ class Search:
|
||||
self.on_target_found = None
|
||||
self._dispatched_targets = set()
|
||||
|
||||
self.spiral_max_legs = 12
|
||||
self.spiral_initial_leg = self.CAM_FOV_METERS
|
||||
self.spiral_leg_increment = self.CAM_FOV_METERS * 0.8
|
||||
hfov = 1.3962634 # 80 deg horizontal FOV
|
||||
self.cam_fov_meters = 2.0 * self.altitude * math.tan(hfov / 2.0)
|
||||
|
||||
self.spiral_max_legs = 40
|
||||
self.spiral_initial_leg = self.cam_fov_meters
|
||||
self.spiral_leg_increment = self.cam_fov_meters * 0.8
|
||||
|
||||
self.lawn_width = 30.0
|
||||
self.lawn_height = 30.0
|
||||
self.lawn_lane_spacing = self.CAM_FOV_METERS * 0.8
|
||||
self.lawn_lanes = 2
|
||||
self.lawn_lane_spacing = self.cam_fov_meters * 0.8
|
||||
self.lawn_lanes = int(self.lawn_width / max(self.lawn_lane_spacing, 0.1)) + 1
|
||||
|
||||
self.levy_max_steps = 20
|
||||
self.levy_field_size = 50.0
|
||||
@@ -159,12 +159,29 @@ class Search:
|
||||
return waypoints
|
||||
|
||||
def _clip_to_geofence(self, waypoints, polygon):
|
||||
warn_dist = getattr(self, 'geofence_warn_dist', 3.0)
|
||||
safe_polygon = []
|
||||
if warn_dist > 0:
|
||||
cx = sum(p[0] for p in polygon) / len(polygon)
|
||||
cy = sum(p[1] for p in polygon) / len(polygon)
|
||||
for x, y in polygon:
|
||||
dx = x - cx
|
||||
dy = y - cy
|
||||
length = math.sqrt(dx*dx + dy*dy)
|
||||
if length > 0:
|
||||
shrink = warn_dist / length
|
||||
safe_polygon.append((x - dx * shrink, y - dy * shrink))
|
||||
else:
|
||||
safe_polygon.append((x, y))
|
||||
else:
|
||||
safe_polygon = polygon
|
||||
|
||||
clipped = []
|
||||
for wx, wy in waypoints:
|
||||
if self._point_in_polygon(wx, wy, polygon):
|
||||
if self._point_in_polygon(wx, wy, safe_polygon):
|
||||
clipped.append((wx, wy))
|
||||
else:
|
||||
nearest = self._nearest_point_on_polygon(wx, wy, polygon)
|
||||
nearest = self._nearest_point_on_polygon(wx, wy, safe_polygon)
|
||||
if not clipped or distance_2d(clipped[-1], nearest) > 0.5:
|
||||
clipped.append(nearest)
|
||||
return clipped
|
||||
@@ -324,10 +341,13 @@ class Search:
|
||||
IMG_CX, IMG_CY = IMG_W / 2, IMG_H / 2
|
||||
HFOV = 1.3962634 # 80° horizontal FOV
|
||||
|
||||
DESCENT_STEP = 1.0
|
||||
LAND_ALT = 1.5
|
||||
self.ctrl.update_state()
|
||||
current_alt = self.ctrl.altitude
|
||||
|
||||
DESCENT_STEP = max(0.5, current_alt * 0.25)
|
||||
LAND_ALT = min(1.0, current_alt - 0.2)
|
||||
CENTER_PX = 50
|
||||
MAX_CORRECTIONS = 30
|
||||
MAX_CORRECTIONS = 200
|
||||
MAX_LOST = 30
|
||||
GAIN = 0.35
|
||||
|
||||
@@ -365,12 +385,16 @@ class Search:
|
||||
|
||||
def center_over_target(self, target_id):
|
||||
print(f"\n[SEARCH] ===== CENTERING OVER TARGET {target_id} =====")
|
||||
if hasattr(self, 'uav_min_cms'):
|
||||
print(f"[SEARCH] Reducing speed to minimum ({self.uav_min_cms} cm/s) for centering.")
|
||||
self.ctrl.configure_speed_limits(wpnav_speed=self.uav_min_cms, loit_speed=self.uav_min_cms)
|
||||
|
||||
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_CORRECTIONS = 200
|
||||
MAX_LOST = 30
|
||||
GAIN = 0.35
|
||||
|
||||
@@ -442,15 +466,22 @@ class Search:
|
||||
alt = max(self.ctrl.altitude, 0.5)
|
||||
ground_w = 2.0 * alt * math.tan(hfov / 2.0)
|
||||
m_per_px = ground_w / img_w
|
||||
|
||||
# Calculate movement in the UAV's body frame
|
||||
correction_forward = -err_y * m_per_px * gain
|
||||
correction_right = err_x * m_per_px * gain
|
||||
|
||||
# Camera pitch-90° inverts image axes relative to NED
|
||||
correction_y = -err_x * m_per_px * gain
|
||||
correction_x = -err_y * m_per_px * gain
|
||||
self.ctrl.update_state()
|
||||
yaw = self.ctrl.current_yaw
|
||||
|
||||
# Rotate body correction (Forward, Right) into world NED (North, East)
|
||||
correction_n = correction_forward * math.cos(yaw) - correction_right * math.sin(yaw)
|
||||
correction_e = correction_forward * math.sin(yaw) + correction_right * math.cos(yaw)
|
||||
|
||||
cur = self.ctrl.get_local_position()
|
||||
self.ctrl.move_local_ned(
|
||||
cur['x'] + correction_x,
|
||||
cur['y'] + correction_y,
|
||||
cur['x'] + correction_n,
|
||||
cur['y'] + correction_e,
|
||||
-target_alt)
|
||||
sleep(0.5)
|
||||
|
||||
@@ -460,8 +491,14 @@ class Search:
|
||||
return centered
|
||||
|
||||
def wait_for_position(self, target_x, target_y, timeout=None):
|
||||
self.ctrl.update_state()
|
||||
pos = self.ctrl.get_local_position()
|
||||
dist_total = distance_2d((pos['x'], pos['y']), (target_x, target_y))
|
||||
|
||||
if timeout is None:
|
||||
timeout = self.MAX_TRAVEL_TIME
|
||||
# Dynamic timeout: 30s base time + 3 seconds per meter
|
||||
timeout = 30.0 + (dist_total * 3.0)
|
||||
|
||||
t0 = time.time()
|
||||
while time.time() - t0 < timeout and self.running:
|
||||
self.ctrl.update_state()
|
||||
|
||||
Reference in New Issue
Block a user