10. 비행중 각도만 변경
import cv2
import numpy as np
import time
from djitellopy import Tello
# ===== 설정 =====
TARGET_IDS = {7, 8}
MARKER_LENGTH_M = 0.20 # 20cm = 0.20m
# 종료 시 자동착륙(원인분리용). 필요하면 True로
AUTO_LAND_ON_EXIT = False
# RC 전송 주기(초) - 0.05 = 20Hz 정도
RC_PERIOD = 0.05
# yaw 제어 파라미터
YAW_DEADBAND_DEG = 2.0 # 목표: ±2도 이내
YAW_KP = 2.0 # 비례게인 (deg -> rc)
MAX_YAW_RC = 40 # yaw RC 최대치 (0~100 권장 범위 내에서)
# ===== 카메라 파라미터 =====
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)
# ===== Tello 연결 =====
tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")
tello.streamon()
frame_read = tello.get_frame_read()
is_flying = False
last_rc_sent = 0.0
print("\n[KEYS] t=takeoff, q=land, ESC=quit")
print("[INFO] Auto yaw control: ON (target yaw_err within ±2deg)\n")
def clamp_int(x, lo, hi):
return int(max(lo, min(hi, x)))
try:
while True:
frame = frame_read.frame
if frame is None:
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, _ = detector.detectMarkers(gray)
yaw_error_deg = None
best_mid = None
best_dist = None
if ids is not None:
ids_flat = ids.flatten()
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS]
if keep_idx:
filtered_corners = [corners[i] for i in keep_idx]
filtered_ids = ids[keep_idx]
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids)
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs
)
# 여러 마커가 보이면 "가장 가까운 마커"를 기준으로 yaw 제어
for i, mid in enumerate(filtered_ids.flatten()):
tvec = tvecs[i][0] # [x,y,z] in meters
x = float(tvec[0])
z = float(tvec[2])
dist = float(np.linalg.norm(tvec))
yaw_deg = float(np.degrees(np.arctan2(x, z)))
# 화면 표시
cv2.putText(frame,
f"ID {int(mid)} dist={dist:.2f}m yaw_err={yaw_deg:+.1f}deg",
(10, 30 + 30 * i),
cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs,
rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
# closest pick
if (best_dist is None) or (dist < best_dist):
best_dist = dist
yaw_error_deg = yaw_deg
best_mid = int(mid)
# 자동 yaw 제어 (이륙했을 때만)
yaw_cmd = 0
if is_flying:
if yaw_error_deg is not None:
# deadband
if abs(yaw_error_deg) <= YAW_DEADBAND_DEG:
yaw_cmd = 0
else:
yaw_cmd = clamp_int(YAW_KP * yaw_error_deg, -MAX_YAW_RC, MAX_YAW_RC)
else:
yaw_cmd = 0 # 마커 없으면 호버
# RC 주기적으로 보내기 (keep-alive + yaw 제어)
now = time.time()
if (now - last_rc_sent) >= RC_PERIOD:
# (lr, fb, ud, yaw)
tello.send_rc_control(0, 0, 0, yaw_cmd)
last_rc_sent = now
# 상태 표시
status = "FLYING" if is_flying else "LANDED"
msg = f"Status:{status} | Keys:t takeoff, q land, ESC quit"
cv2.putText(frame, msg, (10, frame.shape[0] - 45),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
if yaw_error_deg is None:
cv2.putText(frame, f"YawCtrl: yaw_cmd={yaw_cmd:+d} (no marker)",
(10, frame.shape[0] - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
else:
cv2.putText(frame, f"YawCtrl: ID {best_mid} yaw_err={yaw_error_deg:+.1f}deg -> yaw_cmd={yaw_cmd:+d}",
(10, frame.shape[0] - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
cv2.imshow("ArUco (Tello) - auto yaw to ±2deg", frame)
key = cv2.waitKey(1) & 0xFF
if key == 27: # ESC
print("[USER] ESC pressed -> 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
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
tello.land()
is_flying = False
else:
print("[INFO] Already landed")
finally:
print("\n FINALLY ENTERED | is_flying =", is_flying)
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
if AUTO_LAND_ON_EXIT and is_flying:
print("🛬 AUTO_LAND_ON_EXIT=True -> calling tello.land()")
try:
tello.land()
except Exception:
pass
try:
tello.streamoff()
except Exception:
pass
print("cleanup done")
11. 비행중 각도 및 전후진
import cv2
import numpy as np
import time
from djitellopy import Tello
# ===== 설정 =====
TARGET_IDS = {7, 8}
MARKER_LENGTH_M = 0.20 # 20cm
# 종료 시 자동착륙(원인분리용). 필요하면 True로
AUTO_LAND_ON_EXIT = False
# RC 전송 주기(초) - 0.05 = 20Hz 정도
RC_PERIOD = 0.05 # 0.05초. 초당 20번 보낸다.
YAW_DEADBAND_DEG = 2.0 # yaw가 ±2도 이내면 "정렬 완료"
TARGET_Z_M = 1.00 # 마커까지 z를 1.00m 근처로 맞추려고 함
Z_TOL_M = 0.10 # 거리는 ±10cm 허용
# yaw 제어 게인/제한
YAW_KP = 2.0 # yaw_cmd = YAW_KP * yaw_error_deg (deg 단위 오차를 RC 값으로 변환)
MAX_YAW_RC = 40 # max_yaw_rc로 너무 큰 회전 명령을 방지
# 여기는 z축(전후진) 제어.
Z_KP = 80.0 # meters -> rc
MAX_FB_RC = 25 # MAX_FB_RC로 전후 속도 제한
# ===== 카메라 파라미터 =====
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)
# ===== Tello 연결 =====
tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")
tello.streamon()
frame_read = tello.get_frame_read()
# ------비행상태 관련-----
is_flying = False # 현재 드론이 이륙했는지를 기록
last_rc_sent = 0.0 # 마지막으로 send_rc_control을 보낸 기록을 저장히 위한 것. RC_KEEPALIVE_PERIOD만큼 시간이 지나면 다시 보내기 위한것.
# ============================================================
# 상태머신(State Machine) 정의
# - yaw부터 맞추고, yaw가 맞았을 때만 z(거리)로 접근
# ============================================================
MODE_ALIGN = "ALIGN_YAW" # yaw(좌우 회전)만 제어하는 모드
MODE_APPROACH = "MOVE_Z" # 전후(거리)만 제어하는 모드
mode = MODE_ALIGN # 시작은 yaw 정렬부터
print("\n[KEYS] t=takeoff, q=land, ESC=quit")
print("[INFO] Control mode: yaw-align THEN z-move (NO fb when yaw not aligned)\n")
# ============================================================
# 유틸 함수: RC 명령을 정수로 제한(clamp)하기
# ============================================================
def clamp_int(x, lo, hi): # x를 [lo, hi] 범위로 제한한 뒤 int로 변환.
return int(max(lo, min(hi, x)))
# ------------메인 루프 ---------------------
try:
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
# "관측값" 변수들 초기화. 못찾으면 none을 유지.
yaw_error_deg = None # 마커가 좌우로 얼마나 치우쳤는지 각도(도 단위)
z_m = None # 마커까지 전방 거리 tz(미터)
best_mid = None # 여러 마커 중 선택된(가장 가까운) 마커 ID
best_dist = None # 선택 기준(가장 작은 dist)
# --------------------------------------------------------
# 마커가 검출된 경우에만 필터링 및 포즈추정 수행
# --------------------------------------------------------
if ids is not None:
ids_flat = ids.flatten() # ids는 (N,1) -> flatten()으로 (N,) 형태로 변환
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS] # 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
filtered_corners = [corners[i] for i in keep_idx] # 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids) # 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
# 아래는 marker의 2d corner의 위치를 가지고, 카메라가 marker를 어떤 방향(rvecs)으로 보고 있으며, 거리(tvecs)가 얼마나 떨어져있는지를 푼다.
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs
)
# 각 마커에 대해 거리(dist)와 yaw 오차(yaw_error_deg)를 계산해서 표시
for i, mid in enumerate(filtered_ids.flatten()):
# 아래 5줄이 주로 yaw와 거리를 계산하는 부분
tvec = tvecs[i][0]
x = float(tvec[0])
z = float(tvec[2])
dist = float(np.linalg.norm(tvec))
yaw_deg = float(np.degrees(np.arctan2(x, z)))
cv2.putText(frame,f"ID {int(mid)} z={z:.2f}m yaw_err={yaw_deg:+.1f}deg",
(10, 30 + 30 * i), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs,rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
# 가장 가까운 마커로 best_* 업데이트
if (best_dist is None) or (dist < best_dist):
best_dist = dist
best_mid = int(mid)
yaw_error_deg = yaw_deg # 선택된 마커 기준 yaw 오차
z_m = z # 선택된 마커 기준 전방 거리
# ===== 상태머신 전환 규칙 =====
# marker_visible: yaw_error_deg와 z_m이 None이 아니면 "이번 프레임에서 목표 마커를 봤다"
marker_visible = (yaw_error_deg is not None and z_m is not None)
if marker_visible:
if abs(yaw_error_deg) > YAW_DEADBAND_DEG: # yaw 오차가 데드밴드 밖이면 yaw 정렬부터 해야 함
mode = MODE_ALIGN
else:
# yaw가 맞으면 거리 조절 모드 가능
mode = MODE_APPROACH
else:
# 마커 없으면 안전하게 정렬 모드로 두고(실제 명령은 0)
mode = MODE_ALIGN
# ===== 명령 계산 (중요: mode에 따라 한 축만 보냄) =====
yaw_cmd = 0 # 좌우 회전
fb_cmd = 0 # 전후 이동
if is_flying:
if not marker_visible:
# 마커 없으면 호버(0,0,0,0) 유지
yaw_cmd = 0
fb_cmd = 0
else:
if mode == MODE_ALIGN:
# yaw만, fb는 절대 0
fb_cmd = 0
if abs(yaw_error_deg) <= YAW_DEADBAND_DEG:
yaw_cmd = 0
else:
yaw_cmd = clamp_int(YAW_KP * yaw_error_deg, -MAX_YAW_RC, MAX_YAW_RC)
elif mode == MODE_APPROACH:
# 거리만, yaw는 0 (필요하면 아주 작은 보정도 가능하지만 지금은 완전 분리)
yaw_cmd = 0
z_err = z_m - TARGET_Z_M
if abs(z_err) <= Z_TOL_M:
fb_cmd = 0
else:
fb_cmd = clamp_int(Z_KP * z_err, -MAX_FB_RC, MAX_FB_RC)
# RC 전송(keep-alive 포함)
now = time.time()
if (now - last_rc_sent) >= RC_PERIOD:
tello.send_rc_control(0, fb_cmd, 0, yaw_cmd)
last_rc_sent = now
# ===== 화면 표시 =====
status = "FLYING" if is_flying else "LANDED"
cv2.putText(frame,
f"Status:{status} | Mode:{mode} | Keys:t takeoff q land ESC quit",
(10, frame.shape[0] - 70),
cv2.FONT_HERSHEY_SIMPLEX, 0.65, (255, 255, 255), 2)
if not marker_visible:
cv2.putText(frame, "Marker: NONE -> hover (fb=0,yaw=0)",
(10, frame.shape[0] - 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.65, (255, 255, 255), 2)
else:
cv2.putText(frame,
f"Marker ID:{best_mid} yaw_err={yaw_error_deg:+.1f}deg (±{YAW_DEADBAND_DEG}) | z={z_m:.2f}m tgt=1.00±0.05",
(10, frame.shape[0] - 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 2)
cv2.putText(frame,
f"Cmd: fb={fb_cmd:+d} (only in MOVE_Z) | yaw={yaw_cmd:+d} (only in ALIGN_YAW)",
(10, frame.shape[0] - 20),
cv2.FONT_HERSHEY_SIMPLEX, 0.65, (255, 255, 255), 2)
cv2.imshow("ArUco (Tello) - align then move", frame)
key = cv2.waitKey(1) & 0xFF
if key == 27:
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
mode = MODE_ALIGN
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
tello.land()
is_flying = False
else:
print("[INFO] Already landed")
finally:
print("\n🟡 FINALLY ENTERED | is_flying =", is_flying)
try:
cv2.destroyAllWindows()
except Exception:
pass
try:
tello.send_rc_control(0, 0, 0, 0)
except Exception:
pass
if AUTO_LAND_ON_EXIT and is_flying:
try:
tello.land()
except Exception:
pass
try:
tello.streamoff()
except Exception:
pass
print("✅ cleanup done")
12. 박스안에 맞추기
import cv2
import numpy as np
import time
from djitellopy import Tello
# 목표 aruco 마커
TARGET_IDS = {7, 8}
# aruco 마커 길이
MARKER_LENGTH_M = 0.20 # 20cm = 0.20m
# RC keep-alive 주기(초). 0.05s = 20Hz 정도
# tello는 rc제어 모드에서 일정 시간동안 rc명령이 오지 않는다면, 안전을 위해서 착륙을한다.
# 그래서 주기적으로 send_rc_control을 보내서, 나는 제어중이라는 것을 알리기 위한 것.
RC_PERIOD = 0.05 # 0.05초. 초당 20번 보낸다.
# 게이트 조건
YAW_DEADBAND_DEG = 2.0
TARGET_Z_M = 0.75 # 코드와 얼마나 떨어지는지
Z_TOL_GATE_M = 0.10 # ±10cm (게이트 조건)
# 중앙 목표(좌우)
CENTER_BAND_RATIO = 0.10 # 화면 중심 기준 |dx| <= 0.20 * width 이면 OK
# ===== 제어 게인/제한 =====
YAW_KP = 2.5
MAX_YAW_RC = 40
Z_KP = 80.0 # 거리 오차(m)를 RC forward/back 값으로 바꾸는 비례 게인
MAX_FB_RC = 25 # 전진/후진 RC 최대 제한
X_KP = 0.12 # pixel -> rc (환경에 따라 튜닝)
MAX_LR_RC = 25 # 좌우 최대 속도 제한
AUTO_LAND_ON_EXIT = False
# ===== 카메라 파라미터 =====
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 = 렌즈 왜곡 계수
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)
# ===== Tello 연결 =====
tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")
tello.streamon()
frame_read = tello.get_frame_read()
# ------비행상태 관련-----
is_flying = False # 현재 드론이 이륙했는지를 기록
last_rc_sent = 0.0 # 마지막으로 send_rc_control을 보낸 기록을 저장히 위한 것. RC_KEEPALIVE_PERIOD만큼 시간이 지나면 다시 보내기 위한것.
# 상태머신
MODE_ALIGN = "ALIGN_YAW" # yaw만
MODE_Z = "MOVE_Z" # 전진/후진만
MODE_CENTER = "CENTER_X" # 좌/우만
mode = MODE_ALIGN
print("\n[KEYS] t=takeoff, q=land, ESC=quit")
print("[INFO] Flow: ALIGN_YAW -> MOVE_Z -> CENTER_X (only when yaw±2 & z±10cm)\n")
def clamp_int(x, lo, hi):
return int(max(lo, min(hi, x)))
try:
while True:
frame = frame_read.frame # 현재의 최신 frame을 가져온다.
if frame is None: # 만약 프레임이 준비되지 않거나, 통신 문제가 존재할때 none이다.
continue
h, w = frame.shape[:2] # 프레임 높이/너비(픽셀)
cx_target = w / 2.0 # 화면 중심 x좌표(픽셀), 중앙 정렬 목표
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # aruco 검출은 일반적으로 그레이스케일에서 수행 ( 속도/안전성에서 유리 )
corners, ids, _ = detector.detectMarkers(gray) # 마커를 검출. corners는 마커별 4개의 코너 좌표. ids는 검출된 마커의 id목록들
yaw_error_deg = None
z_m = None
best_mid = None
best_dist = None
best_marker_cx = None # 마커 중심 x (픽셀)
marker_visible = False
if ids is not None:# ids가 none이라면 marker를 하나도 못찾은 상태이다.
ids_flat = ids.flatten()# ids는 ( N,1) 형태라서 다루기 불편. 그리서 ( N, )으로 만들어줌. 그러면 [ 7,8,9]와 같은 형태가 됨
keep_idx = [i for i, mid in enumerate(ids_flat) if int(mid) in TARGET_IDS]# 7 또는 8만 남기기. enumerate를 하면 (인덱스,해당 인덱스의 값)이 나온다.
if keep_idx: # keep_idx가 비어있지 않은 경우. 즉, 목표하는 id를 하나라도 찾은 경우
marker_visible = True
filtered_corners = [corners[i] for i in keep_idx]# 목표하는 id의 인덱스만 남긴 corners를 새로 만든다
filtered_ids = ids[keep_idx] # 목표하는 id의 목표 인덱스만 골라낸다.
cv2.aruco.drawDetectedMarkers(frame, filtered_corners, filtered_ids)# 검출된 마커의 테두리와 ID를 프레임 위에 그려줌.
# 아래는 marker의 2d corner의 위치를 가지고, 카메라가 marker를 어떤 방향(rvecs)으로 보고 있으며, 거리(tvecs)가 얼마나 떨어져있는지를 푼다.
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
filtered_corners, MARKER_LENGTH_M, cameraMatrix, distCoeffs
)
# 여러 마커면 가장 가까운 마커 기준
for i, mid in enumerate(filtered_ids.flatten()):
tvec = tvecs[i][0]
x = float(tvec[0])
z = float(tvec[2])
dist = float(np.linalg.norm(tvec))
yaw_deg = float(np.degrees(np.arctan2(x, z)))
# 마커 중심 x(픽셀) 계산: corners(4,2)의 평균
pts = filtered_corners[i].reshape(4, 2)
marker_cx = float(np.mean(pts[:, 0]))
# 화면 표시(디버그)
cv2.putText(frame,
f"ID {int(mid)} z={z:.2f}m yaw_err={yaw_deg:+.1f}deg cx={marker_cx:.0f}px",
(10, 30 + 30 * i),
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs,
rvecs[i], tvecs[i], MARKER_LENGTH_M * 0.5)
if (best_dist is None) or (dist < best_dist):
best_dist = dist
best_mid = int(mid)
yaw_error_deg = yaw_deg
z_m = z
best_marker_cx = marker_cx
# ===== 상태(mode) 결정 =====
if not marker_visible:
mode = MODE_ALIGN
else:
# 1) yaw가 틀리면 무조건 yaw부터
if abs(yaw_error_deg) > YAW_DEADBAND_DEG:
mode = MODE_ALIGN
else:
# 2) yaw 맞으면 거리 게이트(±10cm)
if abs(z_m - TARGET_Z_M) > Z_TOL_GATE_M:
mode = MODE_Z
else:
# 3) yaw & 거리 다 만족하면 중앙 정렬
mode = MODE_CENTER
# ===== 명령 계산 (중요: mode에 따라 한 축만) =====
yaw_cmd = 0
fb_cmd = 0
lr_cmd = 0
if is_flying:
if not marker_visible:
yaw_cmd = fb_cmd = lr_cmd = 0
else:
if mode == MODE_ALIGN:
# yaw만
fb_cmd = 0
lr_cmd = 0
if abs(yaw_error_deg) <= YAW_DEADBAND_DEG:
yaw_cmd = 0
else:
yaw_cmd = clamp_int(YAW_KP * yaw_error_deg, -MAX_YAW_RC, MAX_YAW_RC)
elif mode == MODE_Z:
# 거리만
yaw_cmd = 0
lr_cmd = 0
z_err = z_m - TARGET_Z_M
# 여기서는 "게이트 밖"일 때만 들어오므로 tolerance를 조금 촘촘히 잡아도 됨(원하면 ±5cm로 바꿔도 됨)
if abs(z_err) <= 0.05:
fb_cmd = 0
else:
fb_cmd = clamp_int(Z_KP * z_err, -MAX_FB_RC, MAX_FB_RC)
elif mode == MODE_CENTER:
# 중앙(lr)만 — yaw,z는 이미 게이트 만족한 상태에서만 들어옴
yaw_cmd = 0
fb_cmd = 0
dx = best_marker_cx - cx_target # +면 오른쪽에 있음
band = CENTER_BAND_RATIO * w # |dx| <= band 이면 OK
if abs(dx) <= band:
lr_cmd = 0
else:
# +dx(오른쪽)면 오른쪽으로 이동(lr +)해서 마커를 가운데로 끌어오려는 형태
# 만약 방향이 반대로 느껴지면 부호만 바꾸면 됨: lr_cmd = -...
lr_cmd = clamp_int(X_KP * dx, -MAX_LR_RC, MAX_LR_RC)
# RC 전송(keep-alive 포함)
now = time.time()
if (now - last_rc_sent) >= RC_PERIOD:
# (lr, fb, ud, yaw) ud=0 고정
tello.send_rc_control(lr_cmd, fb_cmd, 0, yaw_cmd)
last_rc_sent = now
# ===== 중앙 밴드 시각화 =====
band = CENTER_BAND_RATIO * w
left = int(cx_target - band)
right = int(cx_target + band)
cv2.line(frame, (left, 0), (left, h), (255, 255, 255), 1)
cv2.line(frame, (right, 0), (right, h), (255, 255, 255), 1)
cv2.line(frame, (int(cx_target), 0), (int(cx_target), h), (255, 255, 255), 1)
status = "FLYING" if is_flying else "LANDED"
cv2.putText(frame,
f"Status:{status} | Mode:{mode} | Keys:t takeoff q land ESC quit",
(10, h - 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.65, (255, 255, 255), 2)
cv2.putText(frame,
f"Cmd: lr={lr_cmd:+d} fb={fb_cmd:+d} yaw={yaw_cmd:+d} | Center band: ±{int(CENTER_BAND_RATIO*100)}% width",
(10, h - 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
cv2.imshow("ArUco (Tello) - yaw/z gate then center", frame)
key = cv2.waitKey(1) & 0xFF
if key == 27:
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
mode = MODE_ALIGN
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
tello.land()
is_flying = 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
if AUTO_LAND_ON_EXIT and is_flying:
try:
tello.land()
except Exception:
pass
try:
tello.streamoff()
except Exception:
pass'코딩 및 기타' 카테고리의 다른 글
| [isaac sim] ROS2 tutorials 5 (1) | 2026.02.26 |
|---|---|
| [isaac sim] ROS2 tutorials 4 (0) | 2026.02.23 |
| [tello] test5 6 7 8 9 (0) | 2026.01.30 |
| pycharm tello (0) | 2026.01.28 |
| [isaac sim] ROS2 tutorials 3 (0) | 2026.01.16 |