main.py - master - Arkud/BPLA_Controller

main.py
1 413 строк67 KB
Arkud
refactor(hold): настройка параметров удержания и упрощение PID-логики
15 июл 2026, 10:00
15 июл 2026, 10:00975543c
# vision_canmv.py — порт на K230 CanMV: RamNode (автомат SEEK/CENTER/CHASE + AdaptivePID)
import math
import time
import struct
import os

from media.sensor import  *
from media.display import *
from media.media import *
from machine import UART, FPIOA
import nncase_runtime as nn
import ulab.numpy as unp
from drone_config import *


# =============================================================================
#  MAVLink v1 драйвер
# =============================================================================

_STX = 0xFE

_ID_HEARTBEAT            = 0
_ID_ATTITUDE             = 30
_ID_GPS_INT              = 33
_ID_RC_CHANNELS          = 65
_ID_RC_OVERRIDE          = 70
_ID_REQUEST_DATA_STREAM  = 66
_ID_COMMAND_LONG         = 76
_ID_VFR_HUD              = 74

MAV_DATA_STREAM_RC_CHANNELS  = 6
MAV_CMD_SET_MESSAGE_INTERVAL = 511

# CRC_EXTRA значения из MAVLink common.xml
_CRC_EXTRA = {
    _ID_HEARTBEAT:           50,
    _ID_ATTITUDE:            39,
    _ID_GPS_INT:            104,
    _ID_RC_CHANNELS:        118,
    _ID_RC_OVERRIDE:        124,
    _ID_REQUEST_DATA_STREAM:148,
    _ID_COMMAND_LONG:       152,
    77:                     143,  # COMMAND_ACK
    _ID_VFR_HUD:             10,
}


def _crc16(data):
    crc = 0xFFFF
    for b in data:
        tmp = (b ^ crc) & 0xFF
        tmp = (tmp ^ ((tmp << 4) & 0xFF)) & 0xFF
        crc = ((crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    return crc


class MAVLinkRC:
    """Лёгкий MAVLink v1 драйвер для K230 MicroPython."""
    def __init__(self, baud=115200,
                 gcs_sys=42, gcs_comp=190,
                 target_sys=1, target_comp=0):
        fpioa = FPIOA()
        fpioa.set_function(3, FPIOA.UART1_TXD)
        fpioa.set_function(4, FPIOA.UART1_RXD)
        self.uart = UART(UART.UART1, baud, UART.EIGHTBITS, UART.PARITY_NONE, UART.STOPBITS_ONE)
        self.gcs_sys     = gcs_sys
        self.gcs_comp    = gcs_comp
        self.target_sys  = target_sys
        self.target_comp = target_comp
        self._seq        = 0

        # Телеметрия (обновляется в poll())
        self.yaw         = 0.0   # рад
        self.pitch       = 0.0   # рад
        self.roll        = 0.0   # рад
        self.current_alt = 8.0   # м, относительно точки взлёта
        self.alt_smooth  = 8.0   # сглаженная высота для alt-контроллера
        self.vx_raw      = 0.0   # м/с, мировая система — север
        self.vy_raw      = 0.0   # м/с, мировая система — восток
        self.vz_raw      = 0.0   # м/с, NED: + вниз
        self.climb_rate      = 0.0   # м/с, + вверх; считаем сами из d(current_alt)/dt
        self.predicted_climb = 0.0
        self._vfr_raw    = None  # climb из VFR_HUD (на этой сборке всегда 0)
        self._alt_prev   = None  # пред. высота для дифференцирования
        self._alt_prev_t = None  # время пред. высоты, ticks_ms
        self._climb_accel = 0.0
        self._prev_climb_for_accel = 0.0

        self.rc_channels  = [0] * 18
        self.rc_rssi      = 0
        self._rc_last_t   = time.ticks_ms()  # время последнего RC_CHANNELS



        self._buf = bytearray()

    def set_message_interval(self, message_id, interval_us, confirmation=0):
        """COMMAND_LONG MAV_CMD_SET_MESSAGE_INTERVAL — запросить конкретное сообщение по ID.
        Работает на любом порту независимо от SRx-параметров и REQUEST_DATA_STREAM.
        interval_us=-1 отключает поток, 0 — оставляет частоту по умолчанию."""
        payload = struct.pack('<7fHBBB',
            float(message_id),
            float(interval_us),
            0.0, 0.0, 0.0, 0.0, 0.0,
            MAV_CMD_SET_MESSAGE_INTERVAL,
            self.target_sys, self.target_comp,
            confirmation)
        self.uart.write(self._build_frame(_ID_COMMAND_LONG, payload))

    def request_data_stream(self, stream_id, rate_hz=10, start=True):
        """Запросить у FC поток сообщений (REQUEST_DATA_STREAM, id=66).
        Аналог того, что делает Mission Planner при подключении —
        не зависит от привязки SRx-параметров к физическому порту."""
        payload = struct.pack('<HBBBB',
            rate_hz,
            self.target_sys, self.target_comp,
            stream_id,
            1 if start else 0)
        self.uart.write(self._build_frame(_ID_REQUEST_DATA_STREAM, payload))

    def send_rc_override(self, ch, ch6=65535):
        """Отправить RC_CHANNELS_OVERRIDE. ch = [roll, pitch, throttle, yaw] (1100–1900).
        ch6: 2000 = CHASE-сигнал, 65535 = не переопределять."""
        payload = struct.pack('<8HBB',
                int(ch[0]), int(ch[1]), int(ch[2]), int(ch[3]),
                65535, int(ch6), 65535, 65535,
                self.target_sys, self.target_comp)
        self.uart.write(self._build_frame(_ID_RC_OVERRIDE, payload))

    def release_rc_override(self):
        """Снять RC override — управление возвращается пульту пилота."""
        payload = struct.pack('<8HBB',
                0, 0, 0, 0, 0, 0, 0, 0,
                self.target_sys, self.target_comp)
        self.uart.write(self._build_frame(_ID_RC_OVERRIDE, payload))

    def send_heartbeat(self):
        """Отправить HEARTBEAT 1 Гц — FC видит K230 как активный companion."""
        payload = struct.pack('<IBBBBB', 0, 18, 8, 0, 4, 3)
        self.uart.write(self._build_frame(_ID_HEARTBEAT, payload))

    def vision_enabled(self):
        """True если CH8 >= RC_TRIGGER_THRESHOLD и RC-пакет пришёл не позже 1.5 с назад."""
        if time.ticks_diff(time.ticks_ms(), self._rc_last_t) > 1500:
            return False  # RC потерян — безопасный выход
        return self.rc_channels[RC_TRIGGER_CHANNEL - 1] >= RC_TRIGGER_THRESHOLD

    def poll(self, max_bytes=512):
        """Буферный парсер MAVLink-фреймов."""
        data = self.uart.read(max_bytes)
        if data:
            self._buf.extend(data)

        i = 0
        consumed = 0
        n = len(self._buf)
        while i + 7 < n:
            if self._buf[i] != _STX:
                i += 1
                consumed = i
                continue
            plen      = self._buf[i + 1]
            frame_end = i + 8 + plen
            if frame_end > n:
                break  # фрейм ещё не пришёл целиком, ждём следующий poll()

            msg_id = self._buf[i + 5]
            base   = i + 6

            if msg_id in _CRC_EXTRA:
                seq     = self._buf[i + 2]
                sysid   = self._buf[i + 3]
                compid  = self._buf[i + 4]
                payload = bytes(self._buf[base : base + plen])
                crc_src = (bytes([plen, seq, sysid, compid, msg_id])
                           + payload + bytes([_CRC_EXTRA[msg_id]]))
                expected = struct.unpack_from('<H', self._buf, base + plen)[0]
                if _crc16(crc_src) == expected:
                    self._handle_message(msg_id, payload)

            consumed = frame_end
            i = frame_end

        if consumed > 0:
            self._buf = self._buf[consumed:]
        elif len(self._buf) > 2048:  # защита от роста при мусоре на линии
            self._buf = bytearray()

    def _build_frame(self, msg_id, payload):
        length = len(payload)
        seq    = self._seq & 0xFF
        self._seq += 1
        header  = bytes([_STX, length, seq, self.gcs_sys, self.gcs_comp, msg_id])
        crc_src = bytes([length, seq, self.gcs_sys, self.gcs_comp, msg_id])
        crc_src += payload + bytes([_CRC_EXTRA[msg_id]])
        return header + payload + struct.pack('<H', _crc16(crc_src))

    def _handle_message(self, msg_id, payload):
        if msg_id == _ID_ATTITUDE:
            _, r, p, y, _, _, _ = struct.unpack_from('<Iffffff', payload)
            self.roll  = r
            self.pitch = p
            self.yaw   = y
        elif msg_id == _ID_GPS_INT:
            _, _, _, _, rel_alt, vx, vy, vz, _ = struct.unpack_from('<IiiiihhhH', payload)
            self.current_alt = rel_alt / 1000.0
            self.alt_smooth  = self.alt_smooth * 0.8 + self.current_alt * 0.2
            self.vx_raw      = vx / 100.0
            self.vy_raw      = vy / 100.0
            self.vz_raw      = vz / 100.0
            # climb_rate = d(rel_alt)/dt. VFR_HUD.climb непригоден (EK3_SRC1_VELZ=GPS → 0).
            # EMA 0.8/0.2 + клип ±3 м/с: rel_alt переоценивает движение ~×2.
            _now = time.ticks_ms()
            if self._alt_prev is not None:
                _dt = time.ticks_diff(_now, self._alt_prev_t) / 1000.0
                if _dt > 0.02:   # защита от деления на малый dt
                    _raw_climb = (self.current_alt - self._alt_prev) / _dt
                    _raw_climb = max(-3.0, min(3.0, _raw_climb))  # отсечь выбросы EKF
                    # 0.88/0.12: climb из rel_alt шумит ±0.4 на 8 Гц — D-член alt_d
                    # дёргал throttle. Сглаживаем сильнее: D реагирует на тренд.
                    self.climb_rate = self.climb_rate * 0.88 + _raw_climb * 0.12
                    _raw_accel = (self.climb_rate - self._prev_climb_for_accel) / _dt
                    self._climb_accel = (self._climb_accel * (1 - CLIMB_ACCEL_ALPHA)
                                          + _raw_accel * CLIMB_ACCEL_ALPHA)
                    self._prev_climb_for_accel = self.climb_rate
                    self.predicted_climb = self.climb_rate + CLIMB_LEAD_TIME * self._climb_accel
                    self._alt_prev   = self.current_alt
                    self._alt_prev_t = _now
            else:
                self._alt_prev   = self.current_alt
                self._alt_prev_t = _now
        elif msg_id == _ID_RC_CHANNELS:
            vals = struct.unpack_from('<I18HBB', payload)
            for i in range(18):
                self.rc_channels[i] = vals[1 + i]
            self.rc_rssi    = vals[19]
            self._rc_last_t = time.ticks_ms()
        elif msg_id == _ID_VFR_HUD:
            _, _, _, _, _, climb = struct.unpack_from('<ffhHff', payload)
            # VFR_HUD.climb на этой сборке всегда 0 — climb_rate ведёт GPS_INT.
            self._vfr_raw = climb
        elif msg_id == 77:  # COMMAND_ACK
            pass


# ── Состояния конечного автомата ──────────────────────────────────────────────
SEEK   = 'SEEK'    # поиск: дрон не видит цель, вращается и летит вперёд
CENTER = 'CENTER'  # центрирование: цель найдена, выравниваем по горизонту
CHASE  = 'CHASE'   # преследование: цель по центру, летим вперёд на полном питче
HOLD   = 'HOLD'    # удержание: цель достаточно крупная, держим дистанцию по bbox

# =============================================================================
#  AdaptivePIDController
# =============================================================================
class AdaptivePIDController:
    def __init__(self, kp, ki, kd,
                 kp_bounds=(80, 850), kd_bounds=(0.05, 4.0), adapt_rate=0.025,
                 area_scale_divisor=7.0, err_mag_cap=0.4):
        self.kp_base = kp
        self.ki_base = ki
        self.kd_base = kd
        self.kp_min, self.kp_max = kp_bounds
        self.kd_min, self.kd_max = kd_bounds
        self.adapt_rate = adapt_rate
        self.area_scale_divisor = area_scale_divisor
        self.err_mag_cap = err_mag_cap
        self.integral   = 0.0
        self.prev_err   = 0.0
        self._err_buf   = []

    def _kp(self, err_mag, area_norm):
        err_mag_capped = min(err_mag, self.err_mag_cap)
        k = self.kp_base * (1.0 + 1.2 * err_mag_capped) / (1.0 + self.area_scale_divisor * area_norm)
        return max(self.kp_min, min(self.kp_max, k))

    def _kd(self, d_mag, area_norm):
        k = self.kd_base * (1.0 + 1.8 * d_mag) / (1.0 + self.area_scale_divisor * area_norm)
        return max(self.kd_min, min(self.kd_max, k))

    def _ki(self, area_norm):
        return self.ki_base / (1.0 + self.area_scale_divisor * 1.7 * area_norm)

    def _adapt(self, err):
        self._err_buf.append(err)
        if len(self._err_buf) > 20:
            self._err_buf.pop(0)
        if len(self._err_buf) < 8:
            return
        recent = self._err_buf[-8:]
        sign_changes = sum(1 for i in range(1, len(recent))
                           if recent[i] * recent[i - 1] < 0)
        if sign_changes >= 5:
            self.kp_base = max(self.kp_min, self.kp_base * (1.0 - self.adapt_rate))
            self.kd_base = min(self.kd_max, self.kd_base * (1.0 + self.adapt_rate * 0.5))
        elif all(abs(e) > 0.6 for e in recent):
            self.kp_base = min(self.kp_max, self.kp_base * (1.0 + self.adapt_rate * 0.4))

    def update(self, err, dt, area_norm=0.0, max_integral=2.0):
        dt = max(dt, 0.01)
        d_err = (err - self.prev_err) / dt
        self.integral = max(-max_integral, min(max_integral, self.integral + err * dt))
        kp = self._kp(abs(err), area_norm)
        ki = self._ki(area_norm)
        kd = self._kd(abs(d_err), area_norm)
        # _adapt() отключён: постоянно менял kp_base без возврата (дрейф
        # 217→627 за полёт), делал поведение непредсказуемым. _kp/_kd/_ki
        # уже дают временную адаптацию по area/err — этого достаточно.
        # self._adapt(err)
        output = kp * err + ki * self.integral + kd * d_err
        self.prev_err = err
        return output, kp, ki, kd, d_err

    def reset(self, prev_err=0.0):
        self.integral = 0.0
        self.prev_err = prev_err
        self._err_buf = []


def smooth_rc(prev, new, alphas):
    return [int(a * n + (1.0 - a) * p) for p, n, a in zip(prev, new, alphas)]


# =============================================================================
#  Логирование на SD-карту
# =============================================================================
LOG_DIR     = "/sdcard/data"
PID_LOG_DIR = "/sdcard/data/pid_logs"

def _ensure_log_dir():
    try:
        os.stat(LOG_DIR)
    except OSError:
        os.mkdir(LOG_DIR)

def _ensure_pid_log_dir():
    try:
        os.stat(PID_LOG_DIR)
    except OSError:
        os.mkdir(PID_LOG_DIR)

class PidLogger:
    """One file per PID loop. Opens on CH8 rising edge, closes on falling edge."""
    def __init__(self, name, columns):
        self.name    = name
        self.columns = columns
        self.file    = None
        self.path    = None

    def open(self, session_n):
        _ensure_pid_log_dir()
        self.path = "{}/{}_{:04d}.csv".format(PID_LOG_DIR, self.name, session_n)
        self.file = open(self.path, "w")
        self.file.write("t_ms," + ",".join(self.columns) + "\n")

    def write(self, t_ms, *values):
        if self.file is None:
            return
        try:
            row = "{}," + ",".join(["{}"] * len(values))
            self.file.write(row.format(t_ms, *values) + "\n")
        except Exception as e:
            print("PID LOG WRITE ERROR ({}): {}".format(self.name, e))

    def flush(self):
        if self.file is not None:
            try:
                self.file.flush()
            except Exception as e:
                print("PID LOG FLUSH ERROR ({}): {}".format(self.name, e))

    def close(self):
        if self.file is not None:
            try:
                self.file.flush()
                self.file.close()
            except Exception as e:
                print("PID LOG CLOSE ERROR ({}): {}".format(self.name, e))
        self.file = None
        self.path = None

# =============================================================================
#  DroneVision — главный класс
# =============================================================================
class DroneVision:
    def __init__(self):
        # ── Лог-файл ─────────────────────────────────────────────────────────
        _ensure_log_dir()
        self.log_file      = None
        self.log_path      = None
        self._was_vision_on = False
        self._prev_throttle_state = SEEK
        self._just_appeared_throttle = False
        self._hold_target_alt = None  # целевая высота HOLD/CHASE (м), фикс при входе
        self._prev_roll_out = 0
        self._seek_ff_buf = []  # hover_ff за последние кадры SEEK/CENTER

        # ── Камера ───────────────────────────────────────────────────────────
        print("init: sensor...")
        self.sensor = Sensor(width=FRAME_W, height=FRAME_H)
        self.sensor.reset()
        self.sensor.set_framesize(width=FRAME_W, height=FRAME_H)
        self.sensor.set_pixformat(Sensor.RGB565)

        print("init: MediaManager...")
        MediaManager.init()
        if STREAM_IDE:
            Display.init(Display.VIRT, width=FRAME_W, height=FRAME_H,
                         fps=30, to_ide=True)
        print("init: sensor.run()...")
        self.sensor.run()
        print("Sensor")

        # ── MAVLink через UART ───────────────────────────────────────────────
        print("init: MAVLink...")
        self.mav = MAVLinkRC(baud=MAV_BAUD, gcs_sys=MAV_GCS_SYS, gcs_comp=MAV_GCS_COMP,
                             target_sys=MAV_TARGET_SYS, target_comp=MAV_TARGET_COMP)
        self.mav.request_data_stream(MAV_DATA_STREAM_RC_CHANNELS, rate_hz=10)
        self.mav.set_message_interval(_ID_RC_CHANNELS, 100000)
        self.mav.set_message_interval(_ID_VFR_HUD,     100000)
        self.mav.set_message_interval(_ID_GPS_INT,    100000)

        print("init: loading kmodel...")
        self._kpu = nn.kpu()
        self._kpu.load_kmodel(KMODEL_PATH)
        # Letterbox-константы вычисляем заранее, а не каждый кадр
        assert MODEL_INPUT[0] == FRAME_W, \
            "MODEL_INPUT[0] должен совпадать с FRAME_W (паддинг только по высоте)"
        assert MODEL_INPUT[1] >= FRAME_H, \
            "MODEL_INPUT[1] меньше FRAME_H — нечего паддить, нужна другая логика"
        self._pad_h   = (MODEL_INPUT[1] - FRAME_H) // 2
        self._pad_row = bytearray(FRAME_W * 3 * self._pad_h)
        print("init: kmodel ready")

        # ── PID ──────────────────────────────────────────────────────────────
        self.pid_x = AdaptivePIDController(
            kp=PID_KP, ki=PID_KI, kd=PID_KD,
            kp_bounds=PID_KP_BOUNDS, kd_bounds=PID_KD_BOUNDS,
            adapt_rate=PID_ADAPT_RATE,
            area_scale_divisor=AREA_SCALE_DIVISOR,
            err_mag_cap=ERR_MAG_CAP,
        )
        self.adaptive_gains = (PID_KP, PID_KI, PID_KD)

        # ── PID CSV-логгеры ───────────────────────────────────────────────────
        self.pidlog_lateral = PidLogger("pid_lateral", [
            "err", "kp", "ki", "kd", "integral", "d_err", "lateral_output"])
        # self.pidlog_alt = PidLogger("pid_alt", [...])  # включить с USE_GPS_ALT_HOLD
        self.pidlog_hold_area = PidLogger("pid_hold_area", [
            "area_norm", "area_target", "err", "p_term", "i_term", "d_term",
            "v_des"])
        self.pidlog_hold_pitch = PidLogger("pid_hold_pitch", [
            "v_des", "v_forward", "speed_err", "d_err", "pitch_target",
            "pitch_rate_limited"])

        self.pidlog_hold_roll = PidLogger("pid_hold_roll", [
            "err_x", "vel_err_x", "p_term", "d_term", "roll_out"])
        self.pidlog_throttle_vert = PidLogger("pid_throttle_vert", [
            "err_y_raw", "err_y_compensated", "pitch_attitude_rad",
            "vert", "ff", "alt_term", "extra", "thr_final", "d_err_y", "v_target", "derr_smooth", "hover_ff", "climb_rate"])
        self.pidlog_raw_bbox = PidLogger("pid_raw_bbox", [
            "cx_px", "cy_px", "w_px", "h_px", "conf", "frame_w", "frame_h"])
        self.pidlog_pitch_calib = PidLogger("pid_pitch_calib", [
            "err_y", "err_x", "pitch_attitude_rad", "roll_attitude_rad",
            "conf", "area_norm"])
        self._pid_loggers = [self.pidlog_lateral,
                              self.pidlog_hold_area, self.pidlog_hold_pitch,
                              self.pidlog_throttle_vert, self.pidlog_hold_roll]
                              # pidlog_raw_bbox / pidlog_pitch_calib — открывать только при диагностике
        self._pid_session_n = 0

        # ── State machine ─────────────────────────────────────────────────────
        self.state           = SEEK
        self.track_frames    = 0
        self.last_yaw_dir    = 1
        self.post_miss       = False
        self.target_alt      = None
        self.center_x_offset = 0.0

        self.lost_frames     = 0
        self.last_err_x      = 0.0
        self.last_err_y      = 0.0
        self._track_cx    = None
        self._track_cy    = None
        self._track_valid = False
        self.last_area_norm  = 0.001
        self.vel_err_x       = 0.0
        self.vel_err_y       = 0.0

        # ── Телеметрия (фильтрованная локально) ──────────────────────────────
        self.vx_world    = 0.0
        self.vy_world    = 0.0
        self.v_forward   = 0.0
        self.drone_yaw   = 0.0
        self.current_alt = 8.0
        self.alpha_v     = 0.25
        self.alt_integral = 0.0

        self._base_ch = [1500, HOVER_PITCH, HOVER_THROTTLE, 1500]
        self._ch8_capture_prev   = False
        self._ch8_capture_needed = False

        self.prev_rc = [self._base_ch[0], SEEK_PITCH, self._base_ch[2], self._base_ch[3]]
        self._vision_was_enabled = False
        self._first_vision_frame = False

        self._ch6_high       = True
        self.smooth_area     = 0.001
        self.area_integral        = 0.0
        self._hold_roll_integral  = 0.0
        self.prev_area_norm  = 0.001
        self._prev_speed_err = 0.0
        self._prev_err_y     = 0.0
        self._prev_err_y_raw = 0.0
        self._prev_err_y_t   = None
        self._d_err_y_smooth = 0.0
        self._target_visible = False

        # ── Yaw chase: сглаживание err_x и rate-limit выхода ──────────────────
        self._yaw_err_x_smooth = 0.0
        self._prev_yaw_out     = 0
        _saved_ff = self._load_hover_ff()
        self._prev_thr_final     = _saved_ff
        self._hover_ff           = float(_saved_ff)

    # =========================================================================
    #  Сохранение/восстановление hover_ff между запусками
    # =========================================================================
    _HOVER_FF_FILE = "/sdcard/hover_ff.txt"

    def _load_hover_ff(self):
        try:
            with open(self._HOVER_FF_FILE, "r") as f:
                val = float(f.read().strip())
            if HOVER_FF_MIN <= val <= HOVER_FF_MAX:
                return val
        except Exception:
            pass
        return float(HOVER_THROTTLE)

    def _save_hover_ff(self, _val):
        pass

    # =========================================================================
    #  Детектор
    # =========================================================================
    def _detect(self, img):
        # Letterbox 320×240 → 320×320: нулевые строки сверху и снизу
        raw_hwc = bytes(img.to_rgb888())
        padded = bytes(self._pad_row) + raw_hwc + bytes(self._pad_row)

        H, W = MODEL_INPUT[1], MODEL_INPUT[0]
        arr_hwc = unp.frombuffer(padded, dtype=unp.uint8).reshape((1, H, W, 3))
        inp = nn.from_numpy(arr_hwc)
        self._kpu.set_input_tensor(0, inp)
        self._kpu.run()
        raw = self._kpu.get_output_tensor(0)  # [1, 4+nc, N]
        return self._postprocess(raw)

    def _postprocess(self, raw):
        # YOLOv8 layout: [1, 4+nc, N] → cx,cy,w,h + class_scores
        out_np = raw.to_numpy()
        data = out_np.reshape((4 + NUM_CLASSES, -1))
        scores = data[4]
        mask = scores >= CONF_THRESH
        idxs = unp.nonzero(mask)[0]

        objects = []
        for ii in range(len(idxs)):
            i = int(idxs[ii])
            score = float(data[4, i])
            if score != score:                 # NaN guard
                continue
            cx = float(data[0, i])
            cy = float(data[1, i])
            bw = float(data[2, i])
            bh = float(data[3, i])
            if cx != cx or cy != cy or bw != bw or bh != bh:
                continue
            x = int(cx - bw / 2)
            y = int(cy - bh / 2) - self._pad_h
            w = int(bw)
            h = int(bh)
            objects.append({'rect': (x, y, w, h), 'confidence': score, 'classid': 0})

        # Простой NMS
        objects.sort(key=lambda o: o['confidence'], reverse=True)
        kept = []
        for obj in objects:
            x1, y1, w1, h1 = obj['rect']
            drop = False
            for k in kept:
                x2, y2, w2, h2 = k['rect']
                ix    = max(0, min(x1 + w1, x2 + w2) - max(x1, x2))
                iy    = max(0, min(y1 + h1, y2 + h2) - max(y1, y2))
                inter = ix * iy
                union = w1 * h1 + w2 * h2 - inter
                if union > 0 and inter / union > NMS_THRESH:
                    drop = True
                    break
            if not drop:
                kept.append(obj)

        if self._track_valid and kept:
            tcx, tcy = self._track_cx, self._track_cy
            def _dist2(o):
                x, y, w, h = o['rect']
                cx, cy = x + w / 2, y + h / 2
                return (cx - tcx) ** 2 + (cy - tcy) ** 2
            kept.sort(key=_dist2)
        else:
            kept.sort(key=lambda o: o['rect'][2] * o['rect'][3], reverse=True)

        if kept:
            x, y, w, h = kept[0]['rect']
            self._track_cx    = x + w / 2
            self._track_cy    = y + h / 2
            self._track_valid = True

        return kept

    # =========================================================================
    #  Телеметрия — небложирующее чтение MAVLink + фильтрация
    # =========================================================================
    def _poll_telemetry(self):
        self.mav.poll()
        self.drone_yaw   = self.mav.yaw
        self.current_alt = self.mav.current_alt
        vx = self.mav.vx_raw
        vy = self.mav.vy_raw
        self.vx_world = self.alpha_v * vx + (1.0 - self.alpha_v) * self.vx_world
        self.vy_world = self.alpha_v * vy + (1.0 - self.alpha_v) * self.vy_world
        self.v_forward = (self.vx_world * math.cos(self.drone_yaw)
                          + self.vy_world * math.sin(self.drone_yaw))
        if USE_GPS_ALT_HOLD and self.target_alt is not None and self._vision_was_enabled:
            alt_err = self.target_alt - self.current_alt
            self.alt_integral = max(-15.0, min(15.0, self.alt_integral + alt_err * 0.02))

        _rc_fresh = time.ticks_diff(time.ticks_ms(), self.mav._rc_last_t) <= 1500
        ch8_raw = self.mav.rc_channels[7]
        ch8_active = _rc_fresh and ch8_raw != 0 and ch8_raw >= RC_TRIGGER_THRESHOLD
        if ch8_active and not self._ch8_capture_prev:
            self._ch8_capture_needed = True  # rising edge — начинаем захват
        if ch8_active and self._ch8_capture_needed:
            captured = self.mav.rc_channels[0:4]
            if all(900 <= c <= 2100 for c in captured):
                self._base_ch = list(captured)
                _captured_thr = float(captured[2])
                self._hover_ff       = max(HOVER_FF_MIN, _captured_thr)
                self._prev_thr_final = self._hover_ff
                self._first_vision_frame = True
                self._ch8_capture_needed = False
                self.log("CH8 capture OK: roll={} pitch={} thr={} yaw={} hover_ff={:.0f}".format(
                    captured[0], captured[1], captured[2], captured[3], self._hover_ff))
            else:
                self.log("CH8 capture WAIT: {}".format(list(captured)))
        if not ch8_active:
            self._ch8_capture_needed = False
        self._ch8_capture_prev = ch8_active

    # =========================================================================
    #  Логирование
    # =========================================================================
    def _next_log_path(self):
        try:
            existing = os.listdir(LOG_DIR)
        except OSError:
            existing = []
        nums = []
        for name in existing:
            if name.startswith("flight_") and name.endswith(".txt"):
                try:
                    nums.append(int(name[7:-4]))
                except ValueError:
                    pass
        next_n = (max(nums) + 1) if nums else 1
        return "{}/flight_{:04d}.txt".format(LOG_DIR, next_n)

    def _open_log(self):
        self.log_path = self._next_log_path()
        self.log_file = open(self.log_path, "w")
        print("Logging started:", self.log_path)

    def _close_log(self):
        if self.log_file is not None:
            try:
                self.log_file.flush()
                self.log_file.close()
            except Exception as e:
                print("LOG CLOSE ERROR:", e)
            print("Logging stopped:", self.log_path)
        self.log_file = None
        self.log_path = None

    def log(self, msg):
        t = time.ticks_ms()
        line = "[{:>8}ms] {}".format(t, msg)
        print(line)
        if self.log_file is not None:
            try:
                self.log_file.write(line + "\n")
            except Exception as e:
                print("LOG WRITE ERROR:", e)

    # =========================================================================
    #  Вспомогательные функции управления
    # =========================================================================
    def _compute_err(self, rect, width, height):
        x, y, w, h = rect
        cx, cy = x + w / 2, y + h / 2
        err_x = (cx - width / 2) / (width / 2.0)
        err_y = (height / 2 - cy) / (height / 2.0)
        return max(-1.0, min(1.0, err_x)), err_y, (w * h) / (width * height)

    def _alt_correction(self):
        if self.target_alt is None:
            return 0
        err = self.target_alt - self.current_alt
        p = int(KP_ALT * err) if abs(err) >= ALT_DEADBAND else 0
        i = int(KI_ALT * self.alt_integral)
        output = max(-250, min(250, p + i))
        # self.pidlog_alt.write(time.ticks_ms(),
        #     self.target_alt, self.current_alt, err, p, i, self.alt_integral, output)
        return output

    def _compensate_pitch_tilt(self, err_y, pitch_rad):
        if not PITCH_ERRY_COMP_ENABLED or not self._target_visible:
            return err_y
        result = err_y - PITCH_ERRY_COMP_GAIN * pitch_rad
        return max(-1.0, min(1.0, result))

    def _throttle_for_pitch(self, err_y=0.0, extra=0):
        err_y_raw  = err_y
        pitch_rad  = self.mav.pitch
        err_y      = self._compensate_pitch_tilt(err_y_raw, pitch_rad)
        now = time.ticks_ms()
        just_appeared = (self.state != SEEK) and (self._prev_throttle_state == SEEK)
        self._prev_throttle_state = self.state
        self._just_appeared_throttle = just_appeared
        if self._prev_err_y_t is not None and not just_appeared:
            dt_ey   = max(time.ticks_diff(now, self._prev_err_y_t) / 1000.0, 0.01)
            d_err_y = (err_y_raw - self._prev_err_y_raw) / dt_ey
        else:
            dt_ey   = 0.01
            d_err_y = 0.0
        self._d_err_y_smooth = 0.15 * d_err_y + 0.85 * self._d_err_y_smooth
        d_err_y = self._d_err_y_smooth
        self._prev_err_y     = err_y
        self._prev_err_y_raw = err_y_raw
        self._prev_err_y_t   = now

        baro_d = KD_BARO_THROTTLE * self.mav.climb_rate  # climb>0 → baro_d>0 → тормозим подъём
        # vert_visual — чистый P по err_y для адаптации hover_ff (без baro_d и D-шума)
        if abs(err_y) < ERR_Y_DEADBAND:
            vert_visual = 0
        else:
            _e_vis      = (err_y - ERR_Y_OFFSET) - (ERR_Y_DEADBAND if (err_y - ERR_Y_OFFSET) > 0 else -ERR_Y_DEADBAND)
            vert_visual = max(-150, min(150, int(KP_THROTTLE * _e_vis * 0.9)))

        if ALT_HOLD_MODE == "baro" and self.state in (HOLD, CHASE) and self._hold_target_alt is not None:
            alt_err = self._hold_target_alt - self.mav.alt_smooth
            if abs(alt_err) < ALT_ERR_DEADBAND:
                _alt_err_eff = 0.0
            else:
                _alt_err_eff = alt_err - (ALT_ERR_DEADBAND if alt_err > 0 else -ALT_ERR_DEADBAND)
            # желаемая вертикальная скорость: убывает к нулю по мере
            # приближения к target_alt — торможение начинается заранее
            desired_climb = max(-ALT_V_MAX, min(ALT_V_MAX, _alt_err_eff / ALT_TAU))
            climb_err = desired_climb - self.mav.climb_rate
            if abs(climb_err) < CLIMB_DEADBAND:
                climb_err = 0.0
            else:
                climb_err -= CLIMB_DEADBAND if climb_err > 0 else -CLIMB_DEADBAND
            vert_visual = max(-150, min(150, int(KD_ALT_HOLD_V * climb_err)))
            if self._target_visible and abs(err_y) > ERRY_ALT_DEADBAND:
                _e_alt = err_y - (ERRY_ALT_DEADBAND if err_y > 0 else -ERRY_ALT_DEADBAND)
                self._hold_target_alt += ERRY_TO_ALT_RATE * _e_alt * dt_ey
                self._hold_target_alt = max(self.mav.current_alt - HOLD_ALT_CLAMP,
                                            min(self.mav.current_alt + HOLD_ALT_CLAMP,
                                                self._hold_target_alt))
            vert = max(-150, min(150, int(KD_ALT_HOLD_V * climb_err)))
        else:
            # camera-логика: err_y напрямую правит throttle (откат / SEEK / CENTER)
            if abs(pitch_rad) > THROTTLE_PITCH_LOCK_RAD:
                # при наклоне err_y ложно завышен — газ не добавляем, снижение разрешаем
                if abs(err_y) < ERR_Y_DEADBAND:
                    vert = max(-150, min(150, int(-baro_d * 0.9)))
                else:
                    _e   = (err_y - ERR_Y_OFFSET) - (ERR_Y_DEADBAND if (err_y - ERR_Y_OFFSET) > 0 else -ERR_Y_DEADBAND)
                    vert = max(-150, min(150, int((KP_THROTTLE * _e + KD_THROTTLE * d_err_y - baro_d) * 0.9)))
                    vert = min(0, vert)   # только снижение
            elif abs(err_y) < ERR_Y_DEADBAND:
                vert = max(-150, min(150, int(-baro_d * 0.9)))
            else:
                _e   = (err_y - ERR_Y_OFFSET) - (ERR_Y_DEADBAND if (err_y - ERR_Y_OFFSET) > 0 else -ERR_Y_DEADBAND)
                vert = max(-150, min(150, int((KP_THROTTLE * _e + KD_THROTTLE * d_err_y - baro_d) * 0.9)))

        # hover_ff адаптируется только вне HOLD/CHASE
        _last_vd = getattr(self, '_last_v_des', 0.0)
        _active_approach = (_last_vd > 0.5 and self.v_forward > 0.3)
        if (self._prev_err_y_t is not None
                and self._target_visible
                and abs(vert_visual) < 40
                and not _active_approach
                and self.state not in (HOLD, CHASE)):
            if vert_visual > 10:
                self._hover_ff = max(HOVER_FF_MIN, min(HOVER_FF_MAX,
                    self._hover_ff + HOVER_CLIMB_RATE * dt_ey))
            elif vert_visual < -10:
                self._hover_ff = max(HOVER_FF_MIN, min(HOVER_FF_MAX,
                    self._hover_ff - HOVER_CLIMB_RATE * dt_ey))
            # Запись на SD не чаще раза в 5 с
            _now_s = time.ticks_ms() / 1000.0
            if not hasattr(self, '_last_ff_save_t'):
                self._last_ff_save_t = 0.0
            if _now_s - self._last_ff_save_t > 5.0:
                self._save_hover_ff(self._hover_ff)
                self._last_ff_save_t = _now_s
        if self.state not in (HOLD, CHASE):
            self._seek_ff_buf.append(self._hover_ff)
            if len(self._seek_ff_buf) > 40:
                self._seek_ff_buf.pop(0)
        ff = int(self._hover_ff)
        if self.state == HOLD:
            _pitch_tilt = self.prev_rc[1] - HOVER_PITCH
            if _pitch_tilt > 0:
                _ff_pitch = min(HOLD_PITCH_FF_CAP, int(HOLD_PITCH_FF_GAIN * _pitch_tilt))
                # подавляем feedforward если уже набираем высоту: alt_d уже
                # тормозит, добавлять газ за наклон в этот момент контрпродуктивно
                _cr_now = self.mav.climb_rate
                if _cr_now > CLIMB_DEADBAND:
                    _ff_pitch = max(0, int(_ff_pitch * (1.0 - min(1.0, _cr_now / 0.3))))
                ff += _ff_pitch

        alt    = self._alt_correction() if USE_GPS_ALT_HOLD else 0
        # ceiling не зависит от captured-газа (при стике ~1300 старый предел обрезал hover_ff)
        _thr_ceiling = HOVER_FF_MAX + 80
        target = max(1100, min(_thr_ceiling, ff + vert + alt + extra))
        if just_appeared:
            self._prev_thr_final = ff
        result = self._rate_limit(target, self._prev_thr_final, THROTTLE_SLEW_MAX)
        self._prev_thr_final = result
        self.pidlog_throttle_vert.write(now,
            err_y_raw, err_y, pitch_rad, vert, ff, alt, extra, result, d_err_y, 0.0, self._d_err_y_smooth, self._hover_ff, self.mav.climb_rate)
        return int(result)

    # =========================================================================
    #  HOLD: удержание дистанции
    # =========================================================================
    def _rate_limit(self, target, prev, max_delta):
        if target > prev:
            return min(prev + max_delta, target)
        return max(prev - max_delta, target)

    def _yaw_chase_output(self, err_x, dt):
        self._yaw_err_x_smooth = (ERR_X_YAW_ALPHA * err_x
                                   + (1.0 - ERR_X_YAW_ALPHA) * self._yaw_err_x_smooth)
        if abs(err_x) < YAW_DEADZONE_CHASE:
            p_term  = 0
            d_term  = 0
            yaw_raw = 0
        else:
            p_term  = YAW_CHASE_KP * self._yaw_err_x_smooth
            d_term  = YAW_CHASE_KD * self.vel_err_x
            yaw_raw = int(p_term + d_term)
        yaw_out = int(self._rate_limit(yaw_raw, self._prev_yaw_out, YAW_OUT_MAX_DELTA))
        self._prev_yaw_out = yaw_out
        return yaw_out

    def _hold_roll_output(self, err_x, dt):
        p_term = KP_ROLL_HOLD * err_x
        d_term = -KD_ROLL_HOLD * self.vel_err_x
        _unclamped = p_term + d_term + HOLD_ROLL_KI * self._hold_roll_integral
        if abs(_unclamped) < HOLD_ROLL_MAX:
            self._hold_roll_integral = max(
                -HOLD_ROLL_I_MAX / HOLD_ROLL_KI,
                min( HOLD_ROLL_I_MAX / HOLD_ROLL_KI,
                     self._hold_roll_integral + err_x * dt))
        i_term = HOLD_ROLL_KI * self._hold_roll_integral
        roll_target = max(-HOLD_ROLL_MAX, min(HOLD_ROLL_MAX, p_term + d_term + i_term))
        roll_out = int(self._rate_limit(roll_target, self._prev_roll_out, ROLL_OUT_MAX_DELTA))
        self._prev_roll_out = roll_out
        self.pidlog_hold_roll.write(time.ticks_ms(),
            err_x, self.vel_err_x, p_term, d_term, roll_out)
        return roll_out

    def _pitch_from_speed(self, v_des, dt, max_delta, pmax):
        speed_err = self.v_forward - v_des
        d_err = (speed_err - self._prev_speed_err) / max(dt, 0.01)
        self._prev_speed_err = speed_err
        pitch_target = max(HOLD_PITCH_MIN, min(pmax,
            int(1500 - PITCH_SPEED_KP * speed_err - PITCH_SPEED_KD * d_err)))
        rate   = PITCH_RATE_LIMIT_HOLD_BRAKE if pitch_target < self.prev_rc[1] else max_delta
        result = self._rate_limit(pitch_target, self.prev_rc[1], rate)
        self.pidlog_hold_pitch.write(time.ticks_ms(),
            v_des, self.v_forward, speed_err, d_err, pitch_target, result)
        return result

    def _pitch_max_for_err_y(self, err_y):
        e = abs(err_y)
        if e < ERR_Y_PITCH_CAP_THRESH:
            return CHASE_PITCH
        ratio = min(1.0, (e - ERR_Y_PITCH_CAP_THRESH) / (1.0 - ERR_Y_PITCH_CAP_THRESH))
        return int(CHASE_PITCH - ratio * (CHASE_PITCH - ERR_Y_PITCH_CAP_MIN))

    def _pitch_desired_from_area(self, area_norm, dt):
        area_err = AREA_HOLD - area_norm
        self.area_integral = max(-0.2, min(0.2, self.area_integral + area_err * dt))
        _raw_deriv = (area_norm - self.prev_area_norm) / max(dt, 0.01)
        self._area_deriv_smooth = getattr(self, '_area_deriv_smooth', 0.0)
        self._area_deriv_smooth = 0.15 * _raw_deriv + 0.85 * self._area_deriv_smooth
        area_deriv = self._area_deriv_smooth
        self.prev_area_norm = area_norm
        p_term  =  HOLD_AREA_KP * area_err
        i_term  =  HOLD_AREA_KI * self.area_integral
        d_term  = -HOLD_AREA_KD * area_deriv
        v_des   = p_term + i_term + d_term
        if area_err < HOLD_AREA_DECEL_ZONE:
            v_fwd_cap = HOLD_V_FWD_MAX * max(HOLD_V_FWD_MIN_RATIO,
                                              area_err / HOLD_AREA_DECEL_ZONE)
        else:
            v_fwd_cap = HOLD_V_FWD_MAX
        v_des   = max(-HOLD_V_REV_MAX, min(v_fwd_cap, v_des))
        _v_des_rate_limit = 0.4  # 0.4 м/с за кадр → плавный разгон ~4 с до 1.5 м/с
        if not hasattr(self, '_prev_v_des'):
            self._prev_v_des = v_des
        v_des = max(self._prev_v_des - _v_des_rate_limit * dt,
                    min(self._prev_v_des + _v_des_rate_limit * dt, v_des))
        self._prev_v_des = v_des
        self.pidlog_hold_area.write(time.ticks_ms(),
            area_norm, AREA_HOLD, area_err, p_term, i_term, d_term, v_des)
        return v_des

    # =========================================================================
    #  Основная логика кадра
    # =========================================================================
    def _process_frame(self, img, objects, dt):
        width, height = FRAME_W, FRAME_H
        v_world = math.sqrt(self.vx_world ** 2 + self.vy_world ** 2)
        cx_f, cy_f = width // 2, height // 2
        err_y = 0.0  # 0.0 в SEEK (цель не видна), перезаписывается ниже
        err_x = 0.0
        self._target_visible = bool(objects)

        # ── Рисуем детекции до любых проверок — чтобы видеть bbox всегда ──────
        if objects:
            img.draw_rectangle(width - 38, 0, 36, 36, color=(255, 0, 0), fill=True)
        if DISPLAY:
            img.draw_cross(cx_f, cy_f, color=(255, 255, 0), size=10, thickness=1)
            if objects:
                xd, yd, wd, hd = objects[0]['rect']
                img.draw_rectangle(xd, yd, wd, hd, color=(0, 255, 0), thickness=2)
                img.draw_string_advanced(xd, max(0, yd - 16), 12,
                    "drone {:.2f}".format(objects[0]['confidence']), color=(0, 255, 0))

        # ── Safety: RC-переключатель CH8 ─────────────────────────────────────
        vision_on = self.mav.vision_enabled()
        if not vision_on:
            if self._vision_was_enabled:
                # Выключили vision — полный сброс в SEEK
                self.pid_x.reset()
                self.state           = SEEK
                self.target_alt      = self.current_alt
                self.alt_integral    = 0.0
                self.center_x_offset = 0.0
                self.post_miss       = False
                self.track_frames    = 0
                self.lost_frames     = 0
                self.vel_err_x            = 0.0
                self.vel_err_y            = 0.0
                self.area_integral        = 0.0
                self._hold_roll_integral  = 0.0
                self._d_err_y_smooth      = 0.0
                self.prev_area_norm  = 0.001
                self.smooth_area     = 0.001
                self._prev_speed_err     = 0.0
                self._yaw_err_x_smooth   = 0.0
                self._prev_yaw_out       = 0
                self._track_valid        = False

                # _prev_thr_final не сбрасываем — slew плавно вернёт к hover_ff
                self._hover_ff = max(HOVER_FF_MIN, min(HOVER_FF_MAX, self._hover_ff))
                self._vision_was_enabled = False
            # Отпускаем все RC-каналы включая ch6
            self.mav.send_rc_override([0, 0, 0, 0], ch6=0)
            return

        if not self._vision_was_enabled:
            # Включили vision — сброс PID перед стартом
            self.pid_x.reset()
            self.state        = SEEK
            self.target_alt   = self.current_alt  # зафиксировать высоту сразу
            self.alt_integral = 0.0
            self._track_valid = False
            self._vision_was_enabled = True

        # ── Обнаружен объект ─────────────────────────────────────────────────
        if objects:
            _bx, _by, _bw, _bh = objects[0]['rect']
            self.pidlog_raw_bbox.write(time.ticks_ms(),
                _bx + _bw / 2, _by + _bh / 2, _bw, _bh,
                objects[0]['confidence'], width, height)
            err_x, err_y, area_norm = self._compute_err(objects[0]['rect'], width, height)
            # медиана из 3 кадров: давит одиночные выбросы без асимметрии EMA
            self._area_buf = getattr(self, '_area_buf', [area_norm, area_norm, area_norm])
            self._area_buf.append(area_norm)
            if len(self._area_buf) > 3:
                self._area_buf.pop(0)
            _sorted = sorted(self._area_buf)
            area_norm = _sorted[1]  # медиана из 3
            self.smooth_area = area_norm

            if self.lost_frames == 0:
                _dt_v = max(dt, 0.03)  # клип dt для производной: спайки при двух кадрах за <10мс
                self.vel_err_x = (PRED_VEL_ALPHA * (err_x - self.last_err_x) / _dt_v
                                  + (1.0 - PRED_VEL_ALPHA) * self.vel_err_x)
                self.vel_err_x = max(-YAW_VEL_ERR_X_MAX, min(YAW_VEL_ERR_X_MAX, self.vel_err_x))
                self.vel_err_y = (PRED_VEL_ALPHA * (err_y - self.last_err_y) / _dt_v
                                  + (1.0 - PRED_VEL_ALPHA) * self.vel_err_y)
            else:
                self.vel_err_x = 0.0
                self.vel_err_y = 0.0

            self.last_err_x     = err_x
            self.last_err_y     = err_y
            self.last_area_norm = area_norm
            self.lost_frames    = 0

            err_x_pid = (err_x - self.center_x_offset) if self.state == CENTER else err_x
            lateral_f, kp, ki, kd, d_err_pid = self.pid_x.update(err_x_pid, dt, area_norm, MAX_INTEGRAL)
            self.adaptive_gains = (kp, ki, kd)
            lateral = int(lateral_f)
            self.pidlog_lateral.write(time.ticks_ms(),
                err_x_pid, kp, ki, kd, self.pid_x.integral, d_err_pid, lateral_f)

            # SEEK → CENTER
            if self.state == SEEK:
                self.track_frames += 1
                if self.track_frames >= MIN_DETECT_FRAMES:
                    self.track_frames    = 0
                    self.center_x_offset = TURN_LEAD_OFFSET * self.last_yaw_dir
                    self.pid_x.reset(prev_err=err_x - self.center_x_offset)
                    self.prev_rc[3] = 1500
                    self.state = CENTER
                    self.log("SEEK -> CENTER  off={:+.2f} Vw={:.2f}".format(
                        self.center_x_offset, v_world))

            # CENTER → CHASE
            if self.state == CENTER:
                if abs(err_x_pid) < CENTER_THRESH:
                    self.center_x_offset = 0.0
                    self.state = CHASE
                    self.log("CENTER -> CHASE  err_x={:.3f}".format(err_x))

            if self.state in (CENTER, CHASE, HOLD) and lateral != 0:
                self.last_yaw_dir = 1 if lateral > 0 else -1

            # CHASE → HOLD: требуем N кадров подряд с достаточной площадью
            # (подтверждение) И умеренную скорость сближения — иначе дрон
            # входит в HOLD на инерции и дёргает throttle/pitch.
            if self.state == CHASE and area_norm >= HOLD_ENTRY_AREA:
                self._hold_entry_frames = getattr(self, '_hold_entry_frames', 0) + 1
            else:
                self._hold_entry_frames = 0
            if (self.state == CHASE
                    and self._hold_entry_frames >= 2
                    and abs(self.v_forward) < 0.4):  # ждём почти нулевой скорости перед входом
                self.area_integral        = 0.0
                self._hold_roll_integral  = 0.0
                self._d_err_y_smooth      = 0.0
                self.prev_area_norm  = area_norm
                self._prev_speed_err = 0.0
                self._prev_v_des     = 0.0  # сброс rate-limit v_des при входе в HOLD
                self.prev_rc[3]          = 1500
                self._prev_yaw_out       = 0
                self._hold_roll_integral = 0.0
                self._hold_entry_frames  = 0
                if len(self._seek_ff_buf) >= 5:
                    self._hover_ff = sum(self._seek_ff_buf) / len(self._seek_ff_buf)
                self._hold_target_alt    = self.mav.current_alt
                self.state = HOLD
                self.log("CHASE -> HOLD  A={:.3f} Vf={:.2f}".format(area_norm, self.v_forward))

            if self.state == HOLD and area_norm < HOLD_EXIT_AREA:
                self._hold_exit_frames = getattr(self, '_hold_exit_frames', 0) + 1
            else:
                self._hold_exit_frames = 0
            if self.state == HOLD and self._hold_exit_frames >= 2:
                self._hold_exit_frames = 0
                self._prev_speed_err = 0.0
                self.state = CHASE
                self.log("HOLD -> CHASE  A={:.3f} (target receding)".format(area_norm))

            # ── RC-выходы по состоянию ────────────────────────────────────────
            if self.state == SEEK:
                roll  = self._base_ch[0]
                if self.post_miss and self.track_frames == 0:
                    yaw_target = max(1200, min(1800, self._base_ch[3] + SEEK_YAW_RATE * self.last_yaw_dir))
                    yaw   = int(self._rate_limit(yaw_target, self.prev_rc[3], SEEK_YAW_MAX_DELTA))
                    label = "SEEK yaw:{}  conf:{}/{}".format(
                        'R' if self.last_yaw_dir > 0 else 'L',
                        self.track_frames, MIN_DETECT_FRAMES)
                else:
                    yaw   = self._base_ch[3]
                    label = "SEEK->  conf:{}/{}".format(self.track_frames, MIN_DETECT_FRAMES)
                pitch  = self._base_ch[1]
                thr    = self._throttle_for_pitch()
                alphas = ALPHA_SEEK
                color  = (200, 200, 0)

            elif self.state == CENTER:
                if abs(err_x_pid) < YAW_DEADZONE_CENTER:
                    yaw = self._base_ch[3]
                else:
                    yaw = max(1200, min(1800, 1500 + int(lateral * YAW_FACTOR_CENTER)))
                roll   = max(1100, min(1800, 1500 + int(lateral * ROLL_FACTOR_CENTER)))
                pitch  = self._base_ch[1]
                thr    = self._throttle_for_pitch(err_y)
                alphas = ALPHA_CENTER
                label  = "CENTER  eX:{:.3f}".format(err_x)
                color  = (255, 165, 0)

            elif self.state == CHASE:
                if USE_GPS_ALT_HOLD and self.target_alt is not None:
                    self.target_alt = max(0.3, self.target_alt + err_y * CHASE_DESCENT_RATE * dt)
                roll    = max(1100, min(1800, 1500 + lateral))
                yaw_out = self._yaw_chase_output(err_x, dt)
                yaw     = max(1200, min(1800, 1500 + yaw_out))
                pitch   = min(CHASE_PITCH, self._pitch_max_for_err_y(err_y))
                thr     = self._throttle_for_pitch(err_y)
                alphas  = ALPHA_CHASE
                label   = "CHASE  A:{:.3f}  Vw:{:.1f}  Zt:{:.1f}".format(
                    area_norm, v_world, self.target_alt or 0)
                color   = (0, 255, 0)

            elif self.state == HOLD:
                v_des   = self._pitch_desired_from_area(area_norm, dt)
                self._last_v_des = v_des
                pmax    = self._pitch_max_for_err_y(err_y)
                pitch   = self._pitch_from_speed(v_des, dt, PITCH_RATE_LIMIT_HOLD, pmax)
                yaw_raw = int(YAW_HOLD_KD * self.vel_err_x)
                yaw_out = int(self._rate_limit(
                    max(-YAW_HOLD_MAX, min(YAW_HOLD_MAX, yaw_raw)),
                    self._prev_yaw_out, YAW_OUT_MAX_DELTA))
                self._prev_yaw_out = yaw_out
                yaw     = max(1200, min(1800, 1500 + yaw_out))
                roll    = max(1100, min(1800, self._base_ch[0] + self._hold_roll_output(err_x, dt)))
                thr     = self._throttle_for_pitch(err_y)
                alphas  = ALPHA_HOLD
                label   = "HOLD  A:{:.3f}  Vd:{:.1f}  Vf:{:.1f}".format(
                    area_norm, v_des, self.v_forward)
                color   = (255, 0, 255)

            else:
                roll  = self._base_ch[0]
                yaw   = self._base_ch[3]
                pitch = self._base_ch[1]
                thr   = self._throttle_for_pitch()
                alphas = ALPHA_SEEK
                label  = "???"
                color  = (128, 128, 128)

            rc = [roll, pitch, thr, yaw]

            if DISPLAY:
                x, y, w, h = objects[0]['rect']
                obj_cx = int(x + w / 2)
                obj_cy = int(y + h / 2)
                img.draw_rectangle(x, y, w, h, color=color, thickness=2)
                img.draw_cross(obj_cx, obj_cy, color=(0, 255, 255), size=20, thickness=1)
                img.draw_line(cx_f, cy_f, obj_cx, obj_cy, color=(0, 255, 255), thickness=1)
                img.draw_string_advanced(10, 30, 14, label, color=color)
                kp_d, ki_d, kd_d = self.adaptive_gains
                img.draw_string_advanced(10, 48, 12,
                    "Kp:{:.0f} Ki:{:.1f} Kd:{:.2f}  Alt:{:.1f}/{:.1f}m".format(
                        kp_d, ki_d, kd_d, self.current_alt, self.target_alt or 0),
                    color=(180, 180, 180))
                yaw_out_d = (self._prev_yaw_out
                             if self.state in (CHASE, HOLD) else 0)
                img.draw_string_advanced(10, 63, 12,
                    "eX:{:+.3f}  eY:{:+.3f}  roll:{}  yaw:{}".format(
                        err_x, err_y, 1500 + lateral, 1500 + yaw_out_d),
                    color=(100, 220, 255))

        # ── Предсказание: объект временно не виден ───────────────────────────
        elif self.state != SEEK and self.lost_frames < MAX_LOST_FRAMES:
            self.lost_frames += 1
            self.vel_err_x *= 0.70
            self.vel_err_y *= 0.70
            err_x     = self.last_err_x
            err_y     = self.last_err_y
            area_norm = self.last_area_norm
            self._target_visible = True  # используем реальный last_err_y, компенсация валидна

            err_x_pid = (err_x - self.center_x_offset) if self.state == CENTER else err_x
            lateral_f, kp, ki, kd, d_err_pid = self.pid_x.update(err_x_pid, dt, area_norm, MAX_INTEGRAL)
            self.adaptive_gains = (kp, ki, kd)
            lateral = int(lateral_f)
            self.pidlog_lateral.write(time.ticks_ms(),
                err_x_pid, kp, ki, kd, self.pid_x.integral, d_err_pid, lateral_f)

            if lateral != 0:
                self.last_yaw_dir = 1 if lateral > 0 else -1

            if self.state == CENTER:
                if abs(err_x_pid) < YAW_DEADZONE_CENTER:
                    yaw = self._base_ch[3]
                else:
                    yaw = max(1200, min(1800, 1500 + int(lateral * YAW_FACTOR_CENTER)))
                roll   = max(1100, min(1800, 1500 + int(lateral * ROLL_FACTOR_CENTER)))
                pitch  = self._base_ch[1]
                thr    = self._throttle_for_pitch(err_y)
                alphas = ALPHA_CENTER
            elif self.state == CHASE:
                if USE_GPS_ALT_HOLD and self.target_alt is not None:
                    self.target_alt = max(0.3, self.target_alt + err_y * CHASE_DESCENT_RATE * dt)
                roll    = max(1100, min(1800, 1500 + lateral))
                yaw_out = self._yaw_chase_output(err_x, dt)
                yaw     = max(1200, min(1800, 1500 + yaw_out))
                pitch   = min(CHASE_PITCH, self._pitch_max_for_err_y(err_y))
                thr     = self._throttle_for_pitch(err_y)
                alphas  = ALPHA_CHASE
            elif self.state == HOLD:
                v_des   = self._pitch_desired_from_area(area_norm, dt)
                self._last_v_des = v_des
                pmax    = self._pitch_max_for_err_y(err_y)
                pitch   = self._pitch_from_speed(v_des, dt, PITCH_RATE_LIMIT_HOLD, pmax)
                yaw_raw = int(YAW_HOLD_KD * self.vel_err_x)
                yaw_out = int(self._rate_limit(
                    max(-YAW_HOLD_MAX, min(YAW_HOLD_MAX, yaw_raw)),
                    self._prev_yaw_out, YAW_OUT_MAX_DELTA))
                self._prev_yaw_out = yaw_out
                yaw     = max(1200, min(1800, 1500 + yaw_out))
                roll    = max(1100, min(1800, self._base_ch[0] + self._hold_roll_output(err_x, dt)))
                thr     = self._throttle_for_pitch(err_y)
                alphas  = ALPHA_HOLD
            else:
                roll  = self._base_ch[0]
                yaw   = self._base_ch[3]
                pitch = self._base_ch[1]
                thr   = self._throttle_for_pitch()
                alphas = ALPHA_SEEK

            rc = [roll, pitch, thr, yaw]

            if DISPLAY:
                img.draw_string_advanced(10, 30, 14,
                    "PRED {}/{}  eX:{:.2f}  Vw:{:.1f}".format(
                        self.lost_frames, MAX_LOST_FRAMES, err_x, v_world),
                    color=(0, 165, 255))

        # ── Потеря цели → SEEK ────────────────────────────────────────────────
        else:
            if self.state != SEEK:
                self.pid_x.reset()
                self.target_alt      = self.current_alt
                self.alt_integral    = 0.0
                self.center_x_offset      = 0.0
                self.post_miss            = True
                self.area_integral        = 0.0
                self._hold_roll_integral  = 0.0
                self._d_err_y_smooth      = 0.0
                self.prev_area_norm  = 0.001
                self.smooth_area     = 0.001
                self._prev_speed_err    = 0.0
                self._yaw_err_x_smooth  = 0.0
                self._prev_yaw_out      = 0
                self._track_valid       = False
                self.log("{} -> SEEK  yaw_dir={:+d}".format(self.state, self.last_yaw_dir))
                self.state = SEEK

            self.track_frames = 0
            self.lost_frames  = 0
            self.vel_err_x    = 0.0
            self.vel_err_y    = 0.0

            roll = self._base_ch[0]
            if self.post_miss:
                yaw_target = max(1200, min(1800, self._base_ch[3] + SEEK_YAW_RATE * self.last_yaw_dir))
                yaw = int(self._rate_limit(yaw_target, self.prev_rc[3], SEEK_YAW_MAX_DELTA))
                if self.v_forward > BRAKE_SPEED_MIN:
                    pitch      = max(BRAKE_PITCH_MIN, int(self._base_ch[1] - BRAKE_KP * self.v_forward))
                    seek_label = "BRAKE  Vf:{:.1f}  p:{}  yaw:{}".format(
                        self.v_forward, pitch, 'R' if self.last_yaw_dir > 0 else 'L')
                else:
                    pitch      = self._base_ch[1]
                    seek_label = "SEEK  yaw:{}  Vw:{:.1f}  Alt:{:.1f}".format(
                        'R' if self.last_yaw_dir > 0 else 'L', v_world, self.current_alt)
            else:
                yaw        = self._base_ch[3]
                pitch      = self._base_ch[1]
                seek_label = "SEEK  Vw:{:.1f}  Alt:{:.1f}".format(v_world, self.current_alt)

            thr    = self._throttle_for_pitch()
            alphas = ALPHA_SEEK

            if DISPLAY:
                img.draw_string_advanced(10, 30, 14, seek_label, color=(0, 80, 255))

            rc = [roll, pitch, thr, yaw]

        # ── Сглаживание и отправка RC ─────────────────────────────────────────
        if self._first_vision_frame:
            self.prev_rc = list(rc)  # первый кадр — мгновенный прыжок без EMA
            self._first_vision_frame = False
        else:
            if self.state == HOLD and rc[1] < self.prev_rc[1]:
                alphas = [alphas[0], ALPHA_HOLD_PITCH_BRAKE, alphas[2], alphas[3]]
            if self._just_appeared_throttle:
                alphas = [alphas[0], alphas[1], ALPHA_THROTTLE_DETECT_EASE, alphas[3]]
            self.prev_rc = smooth_rc(self.prev_rc, rc, alphas)
        # ch6=65535 = не переопределять; CH6-меандр шлётся только из run() каждые 300мс
        self.mav.send_rc_override(self.prev_rc)

        self.pidlog_pitch_calib.write(time.ticks_ms(),
            err_y, err_x, self.mav.pitch, self.mav.roll,
            objects[0]['confidence'] if objects else 0.0,
            self.last_area_norm)
        if self.state == "HOLD":
            _vd = getattr(self, '_last_v_des', 0.0)
            if _vd > 0.05:
                _dist_tag = " >>APP"      # сближается
            elif _vd < -0.05:
                _dist_tag = " <<RET"      # отдаляется
            else:
                _dist_tag = " ==HOLD"     # держит дистанцию
            _centered = (abs(self.last_err_x) < HOLD_CH6_ERR_THRESH
                         and abs(self.last_err_y) < HOLD_CH6_ERR_THRESH)
            _ch6_tag = " CH6+" if _centered else " CH6-"
            # climb_rate в лог чтобы видеть жив ли барометр (cr=...)
            # vfr=? → VFR_HUD не приходит; vfr=+0.00 → FC шлёт, но climb=0
            _vfr_str = " vfr=?" if self.mav._vfr_raw is None else " vfr={:+.2f}".format(self.mav._vfr_raw)
            self.log("[{}] r={} p={} t={} y={} | Vw={:.2f} Vf={:.2f} eX={:+.3f} eY={:+.3f} cr={:+.2f}{}{}{}".format(
                self.state,
                self.prev_rc[0], self.prev_rc[1], self.prev_rc[2], self.prev_rc[3],
                v_world, self.v_forward, err_x, err_y,
                self.mav.climb_rate, _dist_tag, _ch6_tag, _vfr_str))
        else:
            _vfr_str = " vfr=?" if self.mav._vfr_raw is None else " vfr={:+.2f}".format(self.mav._vfr_raw)
            self.log("[{}] r={} p={} t={} y={} | Vw={:.2f} Vf={:.2f} eX={:+.3f} eY={:+.3f} cr={:+.2f}{}".format(
                self.state,
                self.prev_rc[0], self.prev_rc[1], self.prev_rc[2], self.prev_rc[3],
                v_world, self.v_forward, err_x, err_y, self.mav.climb_rate, _vfr_str))

    # =========================================================================
    #  Главный цикл
    # =========================================================================
    def run(self):
        last_t       = time.ticks_ms()
        t_hb         = time.ticks_ms()
        t_stream_req = time.ticks_ms()
        t_ch6        = time.ticks_ms()
        t_log_flush  = time.ticks_ms()
        while True:
            now = time.ticks_ms()
            dt  = max(time.ticks_diff(now, last_t) / 1000.0, 0.01)
            last_t = now
            self._poll_telemetry()

            vision_now = self.mav.vision_enabled()
            if vision_now and not self._was_vision_on:
                self._open_log()
                try:
                    self._pid_session_n = int(self.log_path[-8:-4])
                except Exception:
                    self._pid_session_n += 1
                for pl in self._pid_loggers:
                    pl.open(self._pid_session_n)
            elif not vision_now and self._was_vision_on:
                self._close_log()
                for pl in self._pid_loggers:
                    pl.close()
            self._was_vision_on = vision_now

            if self.log_file is not None and time.ticks_diff(now, t_log_flush) >= 2000:
                try:
                    self.log_file.flush()
                except Exception as e:
                    print("LOG FLUSH ERROR:", e)
                for pl in self._pid_loggers:
                    pl.flush()
                t_log_flush = now

            if time.ticks_diff(now, t_hb) >= 1000:
                self.mav.send_heartbeat()
                t_hb = now

            if time.ticks_diff(now, t_stream_req) >= 1000:
                self.mav.set_message_interval(_ID_RC_CHANNELS, 100000)
                t_stream_req = now

            if time.ticks_diff(now, t_ch6) >= 300:
                _centered = (abs(self.last_err_x) < HOLD_CH6_ERR_THRESH
                     and abs(self.last_err_y) < HOLD_CH6_ERR_THRESH)
                if self.state == HOLD and _centered:
                    self._ch6_high = not self._ch6_high
                    ch6 = 2000 if self._ch6_high else 0
                else:
                    self._ch6_high = True
                    ch6 = 1000
                self.mav.send_rc_override([65535, 65535, 65535, 65535], ch6)
                t_ch6 = now

            img     = self.sensor.snapshot()
            objects = self._detect(img)
            self._process_frame(img, objects, dt)

            if STREAM_IDE:
                Display.show_image(img)

            time.sleep_ms(2)


DroneVision().run()