add KI

drone_pilot/flight.py

@@ -0,0 +1,126 @@
+import time
+import numpy as np
+from typing import List, Tuple, Dict
+from .config import Config
+
+class FlightController:
+    def __init__(self):
+        self.last_sent_rc = [0, 0, 0, 0]
+        self.smooth_face = None
+        self.search_start = time.time()
+        self.status = "INITIALIZING"
+        
+        # Memory for lost targets
+        self.last_target_side = 0 # -1 for left, 1 for right
+        self.lost_time = 0
+
+    def calculate(self, 
+                  faces: List[Tuple], 
+                  is_manual: bool, 
+                  emergency_stop: bool, 
+                  is_locked: bool, 
+                  locked_person: Tuple,
+                  current_height: float, 
+                  target_altitude: float,
+                  tof: int, 
+                  zones: Dict[str, bool], 
+                  zone_scores: Dict[str, float],
+                  manual_rc: Tuple[int, int, int, int]) -> Tuple[Tuple[int, int, int, int], str]:
+        
+        lr, fb, ud, yv = 0, 0, 0, 0
+        
+        # Face smoothing for UI/Visuals
+        if len(faces) > 0:
+            target = max(faces, key=lambda f: f[2] * f[3])
+            if self.smooth_face is None: self.smooth_face = target
+            else:
+                self.smooth_face = tuple(int(self.smooth_face[i]*0.8 + target[i]*0.2) for i in range(4))
+        else:
+            self.smooth_face = None
+
+        if emergency_stop:
+            self.status = "EMERGENCY STOP"
+            return (0, 0, 0, 0), self.status
+
+        # Obstacle Avoidance (always active if flying)
+        center_blocked = zones["CENTER"] or tof < Config.OBSTACLE_TOF_CM
+        if center_blocked:
+            self.status = "AVOIDING OBSTACLE"
+            yv = 80 if zone_scores["LEFT"] < zone_scores["RIGHT"] else -80
+            fb = -30
+            return self._smooth(lr, fb, ud, yv)
+
+        if is_manual:
+            self.status = "MANUAL CONTROL"
+            lr, fb, m_ud, yv = manual_rc
+            if abs(m_ud) > 0: ud = m_ud
+            return self._smooth(lr, fb, ud, yv)
+
+        # AI LOGIC
+        if is_locked:
+            if locked_person is not None:
+                # Target is visible -> Normal Pursuit
+                self.search_start = time.time()
+                self.lost_time = 0
+                (x, y, w, h) = locked_person
+                center_x = x + w // 2
+                err_x = center_x - (Config.WIDTH // 2)
+                
+                # Remember which side it was on
+                self.last_target_side = 1 if err_x > 0 else -1
+                
+                # Rotation (Yaw) - FULL SPEED CAPABLE
+                if abs(err_x) > Config.FACE_DEADZONE:
+                    yv = int(np.clip(Config.YAW_GAIN * err_x, -100, 100))
+                
+                # Forward/Backward pursuit - EXTREME SPEED
+                alignment_factor = max(0.4, 1.0 - (abs(err_x) / Config.FACE_ROT_ONLY))
+                target_fb = int(np.clip(Config.FORWARD_GAIN * (Config.TARGET_PERSON_SIZE - w), -90, 90))
+                fb = int(target_fb * alignment_factor)
+                
+                self.status = "PURSUIT: EXTREME"
+            else:
+                # Target is LOST -> Rapid Search logic
+                if self.lost_time == 0: self.lost_time = time.time()
+                elapsed = time.time() - self.lost_time
+                
+                if elapsed < 10.0: # Search longer and faster
+                    yv = 80 * self.last_target_side 
+                    self.status = f"LOST TARGET: RAPID SCAN {'RIGHT' if self.last_target_side > 0 else 'LEFT'}"
+                else:
+                    self.status = "TARGET LOST: AGGRESSIVE PATROL"
+                    yv = 60
+        
+        elif self.smooth_face is not None:
+            # Face found but not locked
+            (x, y, w, h) = self.smooth_face
+            err_x = (x + w // 2) - (Config.WIDTH // 2)
+            if abs(err_x) > Config.FACE_DEADZONE:
+                yv = int(np.clip(Config.YAW_GAIN * err_x, -80, 80))
+            self.status = "AWAITING LOCK"
+        else:
+            # Patrol mode - faster
+            elapsed = (time.time() - self.search_start) % 6.0
+            if elapsed < 2.0:
+                self.status = "PATROL: DASH"
+                fb = 40
+            else:
+                self.status = "PATROL: FAST SPIN"
+                yv = 60
+
+        return self._smooth(lr, fb, ud, yv)
+
+    def _smooth(self, lr, fb, ud, yv):
+        alpha = Config.SMOOTHING_ALPHA
+        slr = int(self.last_sent_rc[0] * (1-alpha) + lr * alpha)
+        sfb = int(self.last_sent_rc[1] * (1-alpha) + fb * alpha)
+        sud = int(self.last_sent_rc[2] * (1-alpha) + ud * alpha)
+        syv = int(self.last_sent_rc[3] * (1-alpha) + yv * alpha)
+        
+        if abs(slr) < 2: slr = 0
+        if abs(sfb) < 2: sfb = 0
+        if abs(sud) < 2: sud = 0
+        if abs(syv) < 2: syv = 0
+        
+        self.last_sent_rc = [slr, sfb, sud, syv]
+        return (slr, sfb, sud, syv), self.status