import os
import cv2
import numpy as np
import time
import math
from datetime import datetime
from djitellopy import Tello
# =========================
# ===== 설정 / 파라미터 =====
# =========================
FLOOR_MARKER_ID1_ALIGN = 1 # 시작 정렬 마커
FLOOR_MARKER_ID3_ALIGN = 3 # 2.7m 이동 후 정렬 마커
FLOOR_MARKER_ID5_ALIGN = 2 # 3.6m 이동 후 정렬 마커
RC_PERIOD = 0.05
EMA_ALPHA = 0.35
CENTER_BAND_RATIO_X = 0.10
CENTER_BAND_RATIO_Y = 0.10
MOVE_UP_CM = 130
MOVE_RIGHT_AFTER_ID1_CM = 270 # 2.7m = 270cm
MOVE_RIGHT_AFTER_ID3_CM = 360 # 3.6m = 360cm
# ===== 모든 호버 시간 10초 =====
HOVER_BEFORE_UP_S = 3.0
HOVER_AFTER_ID1_ALIGN_S = 10.0
HOVER_AFTER_ID3_ALIGN_S = 10.0
HOVER_AFTER_ID5_ALIGN_S = 10.0
SEARCH_TIMEOUT_ID1_S = 12.0
SEARCH_TIMEOUT_ID3_S = 12.0
SEARCH_TIMEOUT_ID5_S = 12.0
SEARCH_STABLE_N = 1
# ===== 바닥 yaw 정렬(대각 135도 기준) =====
FLOOR_TARGET_EDGE_DEG = 90.0
FLOOR_TARGET_DIAG_DEG = FLOOR_TARGET_EDGE_DEG + 45.0 # 135deg
YAW_KP = 2.0
MAX_YAW_RC = 30
MIN_YAW_RC = 10
YAW_EXIT_DEG = 3.0
STABLE_N = 8
ALIGN_TIMEOUT_S = 30.0
LOST_TIMEOUT_S = 0.30
YAW_SLEW_STEP = 6
YAW_SIGN = 1
# FB로 X 맞추기 (forward -> marker 오른쪽)
FB_KP = 35.0
MAX_FB_RC = 12
MIN_FB_RC = 6
FB_SLEW_STEP = 3
# LR로 Y 맞추기 (드론 오른쪽 -> 마커 아래)
LR_KP = 35.0
MAX_LR_RC = 12
MIN_LR_RC = 6
LR_SLEW_STEP = 3
LR_SIGN_Y = 1
MOVE_SPEED_CM_S = 30
MOVE_MARGIN_S = 0.5
AUTO_LAND_ON_EXIT = False
# ===== 동영상 저장 설정 =====
VIDEO_SAVE_DIR = "frontcam_recordings"
VIDEO_FPS = 30.0
VIDEO_EXT = ".mp4"
VIDEO_FRAME_INTERVAL = 1.0 / VIDEO_FPS
# 전면 카메라 전환 직후 바로 저장하지 않도록 대기
FRONT_RECORD_DELAY_S = 1.0
# downvision(대개 320x240) 프레임을 잘못 저장하지 않기 위한 최소 크기
FRONT_MIN_W = 640
FRONT_MIN_H = 480
# ===== 카메라 파라미터 =====
cameraMatrix = np.array([
[920.0, 0.0, 480.0],
[ 0.0, 920.0, 360.0],
[ 0.0, 0.0, 1.0]
], dtype=np.float32)
distCoeffs = np.array([-0.18, 0.06, 0.000, 0.000, -0.01], dtype=np.float32).reshape(-1, 1)
# ===== ArUco =====
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, params)
# =========================
# ===== 유틸 =====
# =========================
def clamp_int(x, lo, hi):
return int(max(lo, min(hi, x)))
def ema(prev, x, a):
return x if prev is None else (a * x + (1.0 - a) * prev)
def slew(prev, target, step):
if prev is None:
return target
if target > prev + step:
return prev + step
if target < prev - step:
return prev - step
return target
def parallel_err_deg(angle_deg):
return ((angle_deg + 90.0) % 180.0) - 90.0
def undistort_pts(px_pts_4x2):
pts = px_pts_4x2.reshape(-1, 1, 2).astype(np.float32)
und = cv2.undistortPoints(pts, cameraMatrix, distCoeffs)
return und.reshape(-1, 2)
def safe_set_downvision(tello_obj, enable: bool):
try:
cmd = f"downvision {1 if enable else 0}"
resp = tello_obj.send_command_with_return(cmd)
print(cmd, "->", resp)
return resp
except Exception as e:
print("[WARN] downvision switch failed:", e)
return None
def start_sdk_move(tello_obj, move_func, dist_cm):
try:
tello_obj.send_rc_control(0, 0, 0, 0)
except Exception:
pass
time.sleep(0.1)
try:
tello_obj.set_speed(int(MOVE_SPEED_CM_S))
except Exception as e:
print("[WARN] set_speed failed:", e)
move_func(int(dist_cm))
return time.time() + MOVE_MARGIN_S
def make_video_path(marker_id: int):
os.makedirs(VIDEO_SAVE_DIR, exist_ok=True)
ts = datetime.now().strftime("%Y%m%d%H%M")
base = os.path.join(VIDEO_SAVE_DIR, f"{ts}_id{marker_id}{VIDEO_EXT}")
if not os.path.exists(base):
return base
idx = 1
while True:
alt = os.path.join(VIDEO_SAVE_DIR, f"{ts}_id{marker_id}_{idx}{VIDEO_EXT}")
if not os.path.exists(alt):
return alt
idx += 1
def stop_video_recording():
global video_writer, video_recording_path, video_recording_marker_id, video_recording_size
global last_video_write_t
if video_writer is not None:
try:
video_writer.release()
print(f"[REC] saved -> {video_recording_path}")
except Exception as e:
print("[WARN] release video failed:", e)
video_writer = None
video_recording_path = None
video_recording_marker_id = None
video_recording_size = None
last_video_write_t = 0.0
def arm_front_record(marker_id: int):
global front_record_armed, front_record_arm_t, front_record_target_id
stop_video_recording()
front_record_armed = (marker_id is not None)
front_record_target_id = marker_id
front_record_arm_t = time.time()
def disarm_front_record():
global front_record_armed, front_record_arm_t, front_record_target_id
front_record_armed = False
front_record_arm_t = 0.0
front_record_target_id = None
stop_video_recording()
def update_video_recording(frame_bgr, should_record: bool, marker_id: int):
global video_writer, video_recording_path, video_recording_marker_id, video_recording_size
global last_video_write_t
if (not should_record) or (marker_id is None):
stop_video_recording()
return
h, w = frame_bgr.shape[:2]
curr_size = (w, h)
if (
video_writer is None
or video_recording_marker_id != marker_id
or video_recording_size != curr_size
):
stop_video_recording()
path = make_video_path(marker_id)
fourcc = cv2.VideoWriter_fourcc(*"mp4v")
writer = cv2.VideoWriter(path, fourcc, VIDEO_FPS, curr_size)
if not writer.isOpened():
print("[WARN] video writer open failed:", path)
return
video_writer = writer
video_recording_path = path
video_recording_marker_id = marker_id
video_recording_size = curr_size
last_video_write_t = 0.0
print(f"[REC] start -> {video_recording_path} size={curr_size}")
now = time.time()
# 실제 파일 기록 속도를 VIDEO_FPS로 제한
if last_video_write_t != 0.0 and (now - last_video_write_t) < VIDEO_FRAME_INTERVAL:
return
try:
video_writer.write(frame_bgr)
last_video_write_t = now
except Exception as e:
print("[WARN] video write failed:", e)
stop_video_recording()
def switch_to_front_camera(tello_obj):
global is_downvision_active
safe_set_downvision(tello_obj, False)
is_downvision_active = False
time.sleep(0.3)
def switch_to_down_camera(tello_obj):
global is_downvision_active
disarm_front_record() # 전면 카메라 녹화 종료
safe_set_downvision(tello_obj, True)
is_downvision_active = True
time.sleep(0.3)
# =========================
# ===== 상태머신 =====
# =========================
MODE_IDLE = "IDLE"
MODE_HOVER_PRE_UP = "HOVER_PRE_UP"
MODE_SWITCH_DOWNCAM = "SWITCH_DOWNVISION"
MODE_MOVE_UP = "MOVE_UP_130"
MODE_SEARCH_ID1 = "SEARCH_ID1"
MODE_ALIGN_ID1 = "ALIGN_ID1 (YAW->FB->LR)"
MODE_HOVER_AFTER_ID1 = "HOVER_AFTER_ID1_10"
MODE_MOVE_RIGHT_270 = "MOVE_RIGHT_270"
MODE_SEARCH_ID3 = "SEARCH_ID3"
MODE_ALIGN_ID3 = "ALIGN_ID3 (YAW->FB->LR)"
MODE_HOVER_AFTER_ID3 = "HOVER_AFTER_ID3_10"
MODE_MOVE_RIGHT_360 = "MOVE_RIGHT_360"
MODE_SEARCH_ID5 = "SEARCH_ID5"
MODE_ALIGN_ID5 = "ALIGN_ID5 (YAW->FB->LR)"
MODE_HOVER_AFTER_ID5 = "HOVER_AFTER_ID5_10"
MODE_LAND = "LAND"
mode = MODE_IDLE
# =========================
# ===== Tello 연결 =====
# =========================
tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")
tello.streamon()
frame_read = tello.get_frame_read()
is_flying = False
is_downvision_active = False
last_rc_sent = 0.0
# =========================
# ===== 런타임 상태 =====
# =========================
hover_start_t = None
move_end_t = None
search_start_t = None
search_seen_cnt = 0
search_target_id = FLOOR_MARKER_ID1_ALIGN
search_timeout_s = SEARCH_TIMEOUT_ID1_S
# 정렬 공용 상태
align_start_t = None
last_seen_t = 0.0
yaw_f = None
stable_cnt = 0
target_yaw_cmd = 0
target_fb_cmd = 0
target_lr_cmd = 0
prev_yaw = 0
prev_fb = 0
prev_lr = 0
# 최근 인식한 마커 ID
last_seen_marker_id = None
# 녹화 상태
video_writer = None
video_recording_path = None
video_recording_marker_id = None
video_recording_size = None
last_video_write_t = 0.0
front_record_armed = False
front_record_arm_t = 0.0
front_record_target_id = None
# UI 디버그용
marker_cx = marker_cy = None
control_mode = ""
active_align_id = None
print("\n[KEYS] t=takeoff(start), q=land, ESC=quit")
print("[FLOW]")
print(" Takeoff -> HOVER 10s (front cam, no record) -> downvision ON -> UP 130cm")
print(" SEARCH ID1 -> ALIGN ID1 -> remember ID1 -> HOVER 10s (front cam, record) -> downvision ON -> RIGHT 270cm")
print(" SEARCH ID3 -> ALIGN ID3 -> remember ID3 -> HOVER 10s (front cam, record) -> downvision ON -> RIGHT 360cm")
print(" SEARCH ID5 -> ALIGN ID5 -> remember ID5 -> HOVER 10s (front cam, record) -> LAND\n")
def reset_align_state():
global align_start_t, last_seen_t, yaw_f, stable_cnt
global target_yaw_cmd, target_fb_cmd, target_lr_cmd
global prev_yaw, prev_fb, prev_lr
global marker_cx, marker_cy, control_mode
align_start_t = time.time()
last_seen_t = time.time()
yaw_f = None
stable_cnt = 0
target_yaw_cmd = 0
target_fb_cmd = 0
target_lr_cmd = 0
prev_yaw = prev_fb = prev_lr = 0
marker_cx = marker_cy = None
control_mode = ""
try:
while True:
frame = frame_read.frame
if frame is None:
continue
frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
h, w = frame.shape[:2]
cx = w / 2.0
cy = h / 2.0
# 중앙 박스 밴드 경계
band_x = CENTER_BAND_RATIO_X * w
left_band = int(cx - band_x)
right_band = int(cx + band_x)
band_y = CENTER_BAND_RATIO_Y * h
top_band = int(cy - band_y)
bottom_band = int(cy + band_y)
# =========================
# ===== 마커 검출 =====
# =========================
need_detection = mode in [
MODE_SEARCH_ID1, MODE_ALIGN_ID1,
MODE_SEARCH_ID3, MODE_ALIGN_ID3,
MODE_SEARCH_ID5, MODE_ALIGN_ID5
]
corners = ids = None
id1_visible = False
id1_c4 = None
id3_visible = False
id3_c4 = None
id5_visible = False
id5_c4 = None
if need_detection:
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, _ = detector.detectMarkers(gray)
if ids is not None and len(ids) > 0:
ids_list = [int(x) for x in ids.flatten().tolist()]
if FLOOR_MARKER_ID1_ALIGN in ids_list:
idx = ids_list.index(FLOOR_MARKER_ID1_ALIGN)
id1_visible = True
id1_c4 = corners[idx][0]
if FLOOR_MARKER_ID3_ALIGN in ids_list:
idx = ids_list.index(FLOOR_MARKER_ID3_ALIGN)
id3_visible = True
id3_c4 = corners[idx][0]
if FLOOR_MARKER_ID5_ALIGN in ids_list:
idx = ids_list.index(FLOOR_MARKER_ID5_ALIGN)
id5_visible = True
id5_c4 = corners[idx][0]
# 최근 본 마커 ID 기억
if mode in [MODE_SEARCH_ID1, MODE_ALIGN_ID1] and id1_visible:
last_seen_marker_id = FLOOR_MARKER_ID1_ALIGN
elif mode in [MODE_SEARCH_ID3, MODE_ALIGN_ID3] and id3_visible:
last_seen_marker_id = FLOOR_MARKER_ID3_ALIGN
elif mode in [MODE_SEARCH_ID5, MODE_ALIGN_ID5] and id5_visible:
last_seen_marker_id = FLOOR_MARKER_ID5_ALIGN
# SEARCH에서는 마커 박스 표시
if mode in [MODE_SEARCH_ID1, MODE_SEARCH_ID3, MODE_SEARCH_ID5] and ids is not None:
cv2.aruco.drawDetectedMarkers(frame, corners, ids)
# =========================
# ===== SEARCH (ID1/ID3/ID5) =====
# =========================
if mode in [MODE_SEARCH_ID1, MODE_SEARCH_ID3, MODE_SEARCH_ID5] and is_flying:
if search_start_t is None:
search_start_t = time.time()
if mode == MODE_SEARCH_ID1:
found = id1_visible
elif mode == MODE_SEARCH_ID3:
found = id3_visible
else:
found = id5_visible
search_seen_cnt = search_seen_cnt + 1 if found else 0
if search_seen_cnt >= SEARCH_STABLE_N:
if mode == MODE_SEARCH_ID1:
mode = MODE_ALIGN_ID1
active_align_id = FLOOR_MARKER_ID1_ALIGN
reset_align_state()
elif mode == MODE_SEARCH_ID3:
mode = MODE_ALIGN_ID3
active_align_id = FLOOR_MARKER_ID3_ALIGN
reset_align_state()
else:
mode = MODE_ALIGN_ID5
active_align_id = FLOOR_MARKER_ID5_ALIGN
reset_align_state()
search_start_t = None
search_seen_cnt = 0
if search_start_t is not None and (time.time() - search_start_t) >= search_timeout_s:
print(f"[INFO] ID{search_target_id} not found within {search_timeout_s}s -> LAND")
mode = MODE_LAND
search_start_t = None
search_seen_cnt = 0
# =========================
# ===== ALIGN 공통 (ID1 / ID3 / ID5) =====
# =========================
if mode in [MODE_ALIGN_ID1, MODE_ALIGN_ID3, MODE_ALIGN_ID5] and is_flying:
if align_start_t is not None and (time.time() - align_start_t) >= ALIGN_TIMEOUT_S:
print(f"[INFO] ID{active_align_id} align timeout -> LAND")
mode = MODE_LAND
target_yaw_cmd = target_fb_cmd = target_lr_cmd = 0
else:
if mode == MODE_ALIGN_ID1:
visible = id1_visible
c4 = id1_c4
mid = FLOOR_MARKER_ID1_ALIGN
elif mode == MODE_ALIGN_ID3:
visible = id3_visible
c4 = id3_c4
mid = FLOOR_MARKER_ID3_ALIGN
else:
visible = id5_visible
c4 = id5_c4
mid = FLOOR_MARKER_ID5_ALIGN
if visible and (c4 is not None):
last_seen_t = time.time()
marker_cx = float(np.mean(c4[:, 0]))
marker_cy = float(np.mean(c4[:, 1]))
centered_x = (left_band <= marker_cx <= right_band)
centered_y = (top_band <= marker_cy <= bottom_band)
und = undistort_pts(c4)
tl = und[0]
br = und[2]
v = br - tl
diag_angle = math.degrees(math.atan2(float(v[1]), float(v[0])))
err = parallel_err_deg(diag_angle - FLOOR_TARGET_DIAG_DEG)
yaw_f = ema(yaw_f, err, EMA_ALPHA)
# 우선순위: yaw -> FB(X) -> LR(Y) -> stable
if abs(yaw_f) > YAW_EXIT_DEG:
stable_cnt = 0
raw_yaw = YAW_SIGN * (YAW_KP * yaw_f)
cmd_yaw = clamp_int(raw_yaw, -MAX_YAW_RC, MAX_YAW_RC)
if cmd_yaw != 0 and abs(cmd_yaw) < MIN_YAW_RC:
cmd_yaw = int(math.copysign(MIN_YAW_RC, cmd_yaw))
target_yaw_cmd = cmd_yaw
target_fb_cmd = 0
target_lr_cmd = 0
control_mode = "YAW_ONLY"
elif not centered_x:
stable_cnt = 0
target_yaw_cmd = 0
target_lr_cmd = 0
x_err = (marker_cx - cx)
x_norm = x_err / cx
raw_fb = -FB_KP * x_norm
cmd_fb = clamp_int(raw_fb, -MAX_FB_RC, MAX_FB_RC)
if cmd_fb != 0 and abs(cmd_fb) < MIN_FB_RC:
cmd_fb = int(math.copysign(MIN_FB_RC, cmd_fb))
target_fb_cmd = cmd_fb
target_lr_cmd = 0
control_mode = "FB_ONLY(X)"
elif not centered_y:
stable_cnt = 0
target_yaw_cmd = 0
target_fb_cmd = 0
y_err = (marker_cy - cy) # +면 아래
y_norm = y_err / cy
raw_lr = LR_SIGN_Y * (-LR_KP * y_norm)
cmd_lr = clamp_int(raw_lr, -MAX_LR_RC, MAX_LR_RC)
if cmd_lr != 0 and abs(cmd_lr) < MIN_LR_RC:
cmd_lr = int(math.copysign(MIN_LR_RC, cmd_lr))
target_lr_cmd = cmd_lr
control_mode = "LR_ONLY(Y)"
else:
target_yaw_cmd = 0
target_fb_cmd = 0
target_lr_cmd = 0
stable_cnt += 1
control_mode = "HOLD(STABLE)"
cv2.putText(
frame,
f"ID{mid} ALIGN [{control_mode}] diag={diag_angle:+.1f} yaw_err={yaw_f:+.1f} "
f"yaw={target_yaw_cmd:+d} fb={target_fb_cmd:+d} lr={target_lr_cmd:+d} "
f"cx={marker_cx:.0f} cy={marker_cy:.0f} Xok={int(centered_x)} Yok={int(centered_y)} "
f"stable={stable_cnt}/{STABLE_N}",
(10, 120),
cv2.FONT_HERSHEY_SIMPLEX, 0.60, (0, 255, 255), 2
)
if stable_cnt >= STABLE_N:
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
last_seen_marker_id = mid
switch_to_front_camera(tello)
arm_front_record(mid)
if mode == MODE_ALIGN_ID1:
print("[STATE] ID1 aligned -> remember ID1 -> switch FRONT CAM -> HOVER 10s (record) -> switch DOWN CAM -> RIGHT 270cm -> SEARCH ID3")
mode = MODE_HOVER_AFTER_ID1
hover_start_t = time.time()
elif mode == MODE_ALIGN_ID3:
print("[STATE] ID3 aligned -> remember ID3 -> switch FRONT CAM -> HOVER 10s (record) -> switch DOWN CAM -> RIGHT 360cm -> SEARCH ID5")
mode = MODE_HOVER_AFTER_ID3
hover_start_t = time.time()
else:
print("[STATE] ID5 aligned -> remember ID5 -> switch FRONT CAM -> HOVER 10s (record) -> LAND")
mode = MODE_HOVER_AFTER_ID5
hover_start_t = time.time()
else:
if (time.time() - last_seen_t) > LOST_TIMEOUT_S:
target_yaw_cmd = target_fb_cmd = target_lr_cmd = 0
stable_cnt = 0
# =========================
# ===== TAKEOFF 시퀀스 =====
# =========================
if mode == MODE_HOVER_PRE_UP and is_flying:
if hover_start_t is None:
hover_start_t = time.time()
if (time.time() - hover_start_t) >= HOVER_BEFORE_UP_S:
mode = MODE_SWITCH_DOWNCAM
hover_start_t = None
move_end_t = None
if mode == MODE_SWITCH_DOWNCAM and is_flying:
switch_to_down_camera(tello)
mode = MODE_MOVE_UP
if mode == MODE_MOVE_UP and is_flying:
if move_end_t is None:
try:
move_end_t = start_sdk_move(tello, tello.move_up, MOVE_UP_CM)
except Exception as e:
print("[WARN] move_up failed:", e)
mode = MODE_LAND
else:
if time.time() >= move_end_t:
move_end_t = None
mode = MODE_SEARCH_ID1
search_target_id = FLOOR_MARKER_ID1_ALIGN
search_timeout_s = SEARCH_TIMEOUT_ID1_S
search_start_t = time.time()
search_seen_cnt = 0
# =========================
# ===== ID1 정렬 후: HOVER 10s -> DOWNCAM -> RIGHT 270 -> SEARCH ID3 =====
# =========================
if mode == MODE_HOVER_AFTER_ID1 and is_flying:
if hover_start_t is None:
hover_start_t = time.time()
if (time.time() - hover_start_t) >= HOVER_AFTER_ID1_ALIGN_S:
switch_to_down_camera(tello)
mode = MODE_MOVE_RIGHT_270
hover_start_t = None
move_end_t = None
if mode == MODE_MOVE_RIGHT_270 and is_flying:
if move_end_t is None:
try:
move_end_t = start_sdk_move(tello, tello.move_right, MOVE_RIGHT_AFTER_ID1_CM)
except Exception as e:
print("[WARN] move_right(270) failed:", e)
mode = MODE_LAND
else:
if time.time() >= move_end_t:
move_end_t = None
mode = MODE_SEARCH_ID3
search_target_id = FLOOR_MARKER_ID3_ALIGN
search_timeout_s = SEARCH_TIMEOUT_ID3_S
search_start_t = time.time()
search_seen_cnt = 0
# =========================
# ===== ID3 정렬 후: HOVER 10s -> DOWNCAM -> RIGHT 360 -> SEARCH ID5 =====
# =========================
if mode == MODE_HOVER_AFTER_ID3 and is_flying:
if hover_start_t is None:
hover_start_t = time.time()
if (time.time() - hover_start_t) >= HOVER_AFTER_ID3_ALIGN_S:
switch_to_down_camera(tello)
mode = MODE_MOVE_RIGHT_360
hover_start_t = None
move_end_t = None
if mode == MODE_MOVE_RIGHT_360 and is_flying:
if move_end_t is None:
try:
move_end_t = start_sdk_move(tello, tello.move_right, MOVE_RIGHT_AFTER_ID3_CM)
except Exception as e:
print("[WARN] move_right(360) failed:", e)
mode = MODE_LAND
else:
if time.time() >= move_end_t:
move_end_t = None
mode = MODE_SEARCH_ID5
search_target_id = FLOOR_MARKER_ID5_ALIGN
search_timeout_s = SEARCH_TIMEOUT_ID5_S
search_start_t = time.time()
search_seen_cnt = 0
# =========================
# ===== ID5 정렬 후: HOVER 10s -> LAND =====
# =========================
if mode == MODE_HOVER_AFTER_ID5 and is_flying:
if hover_start_t is None:
hover_start_t = time.time()
if (time.time() - hover_start_t) >= HOVER_AFTER_ID5_ALIGN_S:
mode = MODE_LAND
hover_start_t = None
# =========================
# ===== LAND =====
# =========================
if mode == MODE_LAND and is_flying:
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
try:
tello.land()
except Exception:
pass
disarm_front_record()
is_flying = False
mode = MODE_IDLE
safe_set_downvision(tello, False)
is_downvision_active = False
# =========================
# ===== RC 명령 전송 (ALIGN 중만) =====
# =========================
lr_cmd = fb_cmd = yaw_cmd = 0
if is_flying:
now = time.time()
move_in_progress = (
(mode == MODE_MOVE_UP and move_end_t is not None and now < move_end_t)
or (mode == MODE_MOVE_RIGHT_270 and move_end_t is not None and now < move_end_t)
or (mode == MODE_MOVE_RIGHT_360 and move_end_t is not None and now < move_end_t)
)
if not move_in_progress and (now - last_rc_sent) >= RC_PERIOD:
tgt_lr = tgt_fb = tgt_yaw = 0
if mode in [MODE_ALIGN_ID1, MODE_ALIGN_ID3, MODE_ALIGN_ID5]:
tgt_lr = clamp_int(target_lr_cmd, -MAX_LR_RC, MAX_LR_RC)
tgt_fb = clamp_int(target_fb_cmd, -MAX_FB_RC, MAX_FB_RC)
tgt_yaw = clamp_int(target_yaw_cmd, -MAX_YAW_RC, MAX_YAW_RC)
lr_cmd = int(slew(prev_lr, tgt_lr, LR_SLEW_STEP))
fb_cmd = int(slew(prev_fb, tgt_fb, FB_SLEW_STEP))
yaw_cmd = int(slew(prev_yaw, tgt_yaw, YAW_SLEW_STEP))
prev_lr, prev_fb, prev_yaw = lr_cmd, fb_cmd, yaw_cmd
tello.send_rc_control(lr_cmd, fb_cmd, 0, yaw_cmd)
last_rc_sent = now
# =========================
# ===== 녹화 조건 판단 =====
# =========================
record_hover_mode = mode in [MODE_HOVER_AFTER_ID1, MODE_HOVER_AFTER_ID3, MODE_HOVER_AFTER_ID5]
front_frame_ready = (w >= FRONT_MIN_W and h >= FRONT_MIN_H)
should_record_front = (
is_flying
and record_hover_mode
and (not is_downvision_active)
and front_record_armed
and (front_record_target_id is not None)
and ((time.time() - front_record_arm_t) >= FRONT_RECORD_DELAY_S)
and front_frame_ready
)
update_video_recording(frame, should_record_front, front_record_target_id)
# =========================
# ===== HUD / UI =====
# =========================
if not is_flying:
step_text = "STEP: LANDED (press 't' to takeoff)"
else:
if mode == MODE_HOVER_PRE_UP:
step_text = "STEP: HOVER 10s (FRONT CAM before UP 130, NO RECORD)"
elif mode == MODE_SWITCH_DOWNCAM:
step_text = "STEP: SWITCH CAMERA -> DOWNVISION"
elif mode == MODE_MOVE_UP:
step_text = "STEP: MOVE UP 130cm"
elif mode == MODE_SEARCH_ID1:
step_text = "STEP: SEARCH ID1 (DOWNVISION)"
elif mode == MODE_ALIGN_ID1:
step_text = f"STEP: ALIGN ID1 | stable={stable_cnt}/{STABLE_N}"
elif mode == MODE_HOVER_AFTER_ID1:
step_text = "STEP: HOVER 10s (FRONT CAM, RECORD ID1) -> RIGHT 270"
elif mode == MODE_MOVE_RIGHT_270:
step_text = "STEP: MOVE RIGHT 270cm"
elif mode == MODE_SEARCH_ID3:
step_text = "STEP: SEARCH ID3 (DOWNVISION)"
elif mode == MODE_ALIGN_ID3:
step_text = f"STEP: ALIGN ID3 | stable={stable_cnt}/{STABLE_N}"
elif mode == MODE_HOVER_AFTER_ID3:
step_text = "STEP: HOVER 10s (FRONT CAM, RECORD ID3) -> RIGHT 360"
elif mode == MODE_MOVE_RIGHT_360:
step_text = "STEP: MOVE RIGHT 360cm"
elif mode == MODE_SEARCH_ID5:
step_text = "STEP: SEARCH ID5 (DOWNVISION)"
elif mode == MODE_ALIGN_ID5:
step_text = f"STEP: ALIGN ID5 | stable={stable_cnt}/{STABLE_N}"
elif mode == MODE_HOVER_AFTER_ID5:
step_text = "STEP: ID5 ALIGNED -> HOVER 10s (FRONT CAM, RECORD ID5) -> LAND"
elif mode == MODE_LAND:
step_text = "STEP: LANDING"
else:
step_text = f"STEP: {mode}"
cv2.putText(frame, step_text, (10, 170),
cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 200, 255), 3)
# 중앙 박스
cv2.line(frame, (left_band, 0), (left_band, h), (255, 255, 255), 1)
cv2.line(frame, (right_band, 0), (right_band, h), (255, 255, 255), 1)
cv2.line(frame, (int(cx), 0), (int(cx), h), (255, 255, 255), 1)
cv2.line(frame, (0, top_band), (w, top_band), (255, 255, 255), 1)
cv2.line(frame, (0, bottom_band), (w, bottom_band), (255, 255, 255), 1)
cv2.line(frame, (0, int(cy)), (w, int(cy)), (255, 255, 255), 1)
# 마커 중심 표시(정렬 중)
if marker_cx is not None and marker_cy is not None and mode in [MODE_ALIGN_ID1, MODE_ALIGN_ID3, MODE_ALIGN_ID5]:
cv2.circle(frame, (int(marker_cx), int(marker_cy)), 6, (0, 255, 0), -1)
cam_status = "DOWN" if is_downvision_active else "FRONT"
rec_status = "ON" if video_writer is not None else "OFF"
arm_status = "ARMED" if front_record_armed else "DISARMED"
last_id_text = f"id{last_seen_marker_id}" if last_seen_marker_id is not None else "None"
status = "FLYING" if is_flying else "LANDED"
cv2.putText(frame, f"Status:{status} | Cam:{cam_status} | Mode:{mode}",
(10, h - 95), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 2)
cv2.putText(frame, f"Cmd: lr={lr_cmd:+d} fb={fb_cmd:+d} yaw={yaw_cmd:+d}",
(10, h - 60), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 2)
cv2.putText(frame, f"FrontRec:{rec_status} | Arm:{arm_status} | LastSeen:{last_id_text}",
(10, h - 25), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 0), 2)
cv2.imshow("Tello ArUco - Align ID1 -> Right270 -> Align ID3 -> Right360 -> Align ID5", frame)
# =========================
# ===== 키 입력 =====
# =========================
key = cv2.waitKey(1) & 0xFF
if key == 27: # ESC
print("[USER] ESC -> quit")
break
if key == ord('t'):
if not is_flying:
print("[USER] Takeoff requested")
tello.takeoff()
time.sleep(2)
is_flying = True
last_rc_sent = 0.0
prev_lr = prev_fb = prev_yaw = 0
# 초기 상태 초기화
last_seen_marker_id = None
disarm_front_record()
last_video_write_t = 0.0
# 이륙 직후 호버는 전면 카메라
switch_to_front_camera(tello)
mode = MODE_HOVER_PRE_UP
hover_start_t = time.time()
move_end_t = None
# reset search/align
search_start_t = None
search_seen_cnt = 0
search_target_id = FLOOR_MARKER_ID1_ALIGN
search_timeout_s = SEARCH_TIMEOUT_ID1_S
active_align_id = None
yaw_f = None
stable_cnt = 0
target_yaw_cmd = target_fb_cmd = target_lr_cmd = 0
else:
print("[INFO] Already flying")
if key == ord('q'):
if is_flying:
print("[USER] Land requested")
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
try:
tello.land()
except Exception:
pass
disarm_front_record()
is_flying = False
mode = MODE_IDLE
safe_set_downvision(tello, False)
is_downvision_active = False
else:
print("[INFO] Already landed")
finally:
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
disarm_front_record()
if AUTO_LAND_ON_EXIT and is_flying:
try:
tello.land()
except Exception:
pass
try:
safe_set_downvision(tello, False)
except Exception:
pass
try:
tello.streamoff()
except Exception:
pass
try:
tello.end()
except Exception:
pass