fix: return to center

autopilot.py

@@ -9,7 +9,7 @@ random.seed(1)
 class Pilot:
     def __init__(self): pass
     def handle(self, image: Image): pass
-    def act(self) -> float | None: pass
+    def act(self) -> tuple[float, float] | None: pass
 
 class AutoPilot(Pilot):
     prev_image: np.ndarray | None
@@ -89,11 +89,11 @@ class AutoPilot(Pilot):
         for match in good_matches:
             # Координаты точки в первом изображении
             pt1 = kp1[match.queryIdx].pt
-            src_pts.append([pt1[0] - center_x1, pt1[1] - center_y1])
+            src_pts.append([center_x1 - pt1[0], pt1[1] - center_y1])
             
             # Координаты точки во втором изображении
             pt2 = kp2[match.trainIdx].pt
-            dst_pts.append([pt2[0] - center_x2, pt2[1] - center_y2])
+            dst_pts.append([center_x2 - pt2[0], pt2[1] - center_y2])
         
         # Конвертируем в numpy массивы
         src_pts = np.float32(src_pts).reshape(-1, 1, 2)
@@ -125,7 +125,7 @@ class AutoPilot(Pilot):
         tx, ty = H[0, 2], H[1, 2]
         
         # Вычисляем угол поворота
-        angle = np.arctan2(a21, a11)
+        angle = -np.arctan2(a21, a11)
         
         # Вычисляем масштаб
         scale_x = np.sqrt(a11**2 + a21**2)
@@ -150,8 +150,8 @@ class AutoPilot(Pilot):
         scale = transformation_info['scale']
         
         # Координаты уже отцентрированы, поэтому используем их напрямую
-        dx_meters = tx
-        dy_meters = ty
+        dx_meters = ty
+        dy_meters = tx
         
         # Применяем поворот к смещению (учитываем текущий угол БПЛА)
         cos_angle = math.cos(self.angle)
@@ -159,8 +159,8 @@ class AutoPilot(Pilot):
         
         # Поворачиваем смещение в глобальные координаты
         # Обратите внимание: dy_meters инвертирован, так как в изображениях Y направлен вниз
-        dx_global = dx_meters * cos_angle - (-dy_meters) * sin_angle
-        dy_global = dx_meters * sin_angle + (-dy_meters) * cos_angle
+        dx_global = dx_meters * cos_angle - dy_meters * sin_angle
+        dy_global = dx_meters * sin_angle + dy_meters * cos_angle
         
         # Обновляем координаты БПЛА
         self.x += dx_global
@@ -276,7 +276,7 @@ class AutoPilot(Pilot):
         # Обновляем предыдущее изображение
         self.prev_image = current_image
 
-    def act(self) -> float | None:
+    def act(self) -> tuple[float, float] | None:
         """
         Возвращает угол поворота для управления дроном, чтобы он стремился к точке (0, 0).
         Если дрон находится рядом с началом координат (в радиусе 1 метра), возвращает None.
@@ -284,8 +284,8 @@ class AutoPilot(Pilot):
         # Расстояние до цели (0, 0)
         distance_to_target = math.sqrt(self.x**2 + self.y**2)
         
-        # Если дрон находится рядом с целью (в радиусе 1 метра), останавливаемся
-        if distance_to_target < 1.0:
+        # Если дрон находится рядом с целью, останавливаемся
+        if distance_to_target < 10.0:
             return None
         
         # Вычисляем угол к цели
@@ -302,7 +302,7 @@ class AutoPilot(Pilot):
             angle_diff -= 2 * math.pi
 
         # Возвращаем угол поворота (положительный - поворот влево, отрицательный - вправо)
-        return max(min(0.05, angle_diff / 2), -0.05)
+        return max(min(0.1, angle_diff), -0.1), min(10., distance_to_target / 2)
 
     def reset_position(self, x: float = 0.0, y: float = 0.0, angle: float = 0.0):
         """Сбрасывает позицию и угол БПЛА"""
@@ -318,12 +318,12 @@ class RandomPilot(Pilot):
     def __init__(self, velocity: float = 1):
         self.counter = 0
 
-    def act(self) -> float | None:
+    def act(self) -> tuple[float, float] | None:
         self.counter += 1
         if self.counter > 40:
             return None
         
-        return 1 / (self.counter + 20)
+        return 1 / (self.counter + 20), 10.0
 
 # def _test():
 #     randomPilot = RandomPilot()