pycharm tello

1. pycharm --> new project 

 

 

2. interpreter에서 venv를 선택

 

 

3. 새 프로젝트 생성후 Settings/Preferences → Project → Python Interpreter에서 venv가 선택되어 있는지 확인

 

4. 다음을 설치

python -m pip install --upgrade pip
pip install djitellopy opencv-python numpy

 

 

5.

from djitellopy import Tello

tello = Tello()
tello.connect()
print("battery:", tello.get_battery(), "%")

 

 

6.

from djitellopy import Tello
import cv2

tello = Tello()
tello.connect()

tello.streamon()
frame_read = tello.get_frame_read()

while True:
    frame = frame_read.frame
    cv2.imshow("tello", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

tello.streamoff()
cv2.destroyAllWindows()

 

7.

from djitellopy import Tello
import time

tello = Tello()
tello.connect()

tello.takeoff()
time.sleep(2)
tello.land()

 

 

8.

import cv2

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
marker_id = 7
marker_size_px = 800  # 크게 만들수록 인식 쉬움

marker = cv2.aruco.generateImageMarker(aruco_dict, marker_id, marker_size_px)
cv2.imwrite("aruco_7.png", marker)
print("saved: aruco_7.png")

import cv2
from djitellopy import Tello

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(aruco_dict, params)

tello = Tello()
tello.connect()
print("Battery:", tello.get_battery(), "%")

tello.streamon()
frame_read = tello.get_frame_read()

while True:
    frame = frame_read.frame
    if frame is None:
        continue

    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    corners, ids, _ = detector.detectMarkers(gray)

    if ids is not None:
        cv2.aruco.drawDetectedMarkers(frame, corners, ids)
        cv2.putText(frame, f"IDs: {ids.flatten().tolist()}",
                    (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 2)

    cv2.imshow("ArUco test (Tello)", frame)
    if (cv2.waitKey(1) & 0xFF) == 27:  # ESC
        break

tello.streamoff()
cv2.destroyAllWindows()

 

 

 

9.

import cv2
import numpy as np
from djitellopy import Tello

# =========================
# 설정값 (여기만 먼저 조정)
# =========================
MARKER_DICT = cv2.aruco.DICT_4X4_50
MARKER_SIZE_M = 0.15        # 마커 한 변 실제 길이(미터). 예: 15cm -> 0.15
TILT_THRESH_DEG = 5.0       # 이 각도(도) 이상이면 "기울어짐"으로 판정
TARGET_ID = None            # 특정 ID만 쓸 거면 숫자(예: 7). 아니면 None(첫 마커 사용)
CALIB_PATH = "calib.npz"    # 캘리브레이션 파일(있으면 사용). 없으면 대략값 사용

# =========================
# 카메라 내부 파라미터
# =========================
def approx_camera_matrix(w, h, fov_deg=82.0):
    # 캘리브레이션 없을 때 "대략"으로 쓰는 내부파라미터
    f = (w / 2) / np.tan(np.radians(fov_deg / 2))
    cx, cy = w / 2, h / 2
    K = np.array([[f, 0, cx],
                  [0, f, cy],
                  [0, 0, 1]], dtype=np.float32)
    dist = np.zeros((5, 1), dtype=np.float32)
    return K, dist

def load_calib_or_approx(w, h, calib_path=CALIB_PATH):
    try:
        data = np.load(calib_path)
        K = data["K"].astype(np.float32)
        dist = data["dist"].astype(np.float32)
        return K, dist, True
    except Exception:
        K, dist = approx_camera_matrix(w, h)
        return K, dist, False

# =========================
# "절대 기울어짐" 계산 (Up-벡터 방식)
# =========================
def compute_tilt_deg_from_rvec(rvec):
    """
    rvec: estimatePoseSingleMarkers에서 나온 rvec (마커->카메라 회전)
    반환: tilt_deg (마커의 '수평으로 맞춘 up' 대비 카메라 up이 얼마나 꺾였는지)
    """
    R_obj2cam, _ = cv2.Rodrigues(rvec)     # marker(object) -> camera
    R_cam2obj = R_obj2cam.T               # camera -> marker

    # OpenCV 카메라 좌표: x=오른쪽, y=아래, z=앞
    # 카메라의 "위" 방향은 -Y
    up_cam = np.array([0.0, -1.0, 0.0], dtype=np.float32)

    # 카메라 up을 마커 좌표로 변환
    up_obj = R_cam2obj @ up_cam
    up_obj = up_obj / (np.linalg.norm(up_obj) + 1e-9)

    # 마커를 "수평으로(윗변이 수평)" 붙였다고 가정하면,
    # 마커 좌표계에서 '월드 up(중력 up)'을 -Y로 둘 수 있음
    world_up_obj = np.array([0.0, -1.0, 0.0], dtype=np.float32)

    cosang = float(np.clip(np.dot(up_obj, world_up_obj), -1.0, 1.0))
    tilt_deg = float(np.degrees(np.arccos(cosang)))
    return tilt_deg

# =========================
# 메인
# =========================
def main():
    # ArUco 설정
    aruco_dict = cv2.aruco.getPredefinedDictionary(MARKER_DICT)
    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()

    tilt_bias = None  # 절대 0도 보정용(카메라 장착 오프셋). 'b'키로 설정.

    while True:
        frame = frame_read.frame
        if frame is None:
            continue

        h, w = frame.shape[:2]
        K, dist, calib_ok = load_calib_or_approx(w, h, CALIB_PATH)

        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        corners, ids, _ = detector.detectMarkers(gray)

        status_text = "NO MARKER"
        status_color = (0, 0, 255)
        info_lines = [
            f"Calib: {'OK' if calib_ok else 'APPROX'} ({CALIB_PATH})",
            "Keys: ESC=quit, b=set_bias(level), r=reset_bias"
        ]

        chosen_rvec = None
        chosen_tvec = None
        chosen_id = None

        if ids is not None and len(ids) > 0:
            ids_flat = ids.flatten().tolist()
            cv2.aruco.drawDetectedMarkers(frame, corners, ids)

            # 포즈 추정
            rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
                corners, MARKER_SIZE_M, K, dist
            )

            # 사용할 마커 선택
            idx = 0
            if TARGET_ID is not None and TARGET_ID in ids_flat:
                idx = ids_flat.index(TARGET_ID)

            chosen_rvec = rvecs[idx][0]
            chosen_tvec = tvecs[idx][0]
            chosen_id = ids_flat[idx]

            # 축 표시(선택)
            cv2.drawFrameAxes(frame, K, dist, chosen_rvec, chosen_tvec, 0.08)

            # 절대 기울기 계산
            tilt_deg = compute_tilt_deg_from_rvec(chosen_rvec)

            # bias 보정(선택: 카메라 장착 오프셋 제거)
            if tilt_bias is None:
                tilt_corr = tilt_deg
            else:
                tilt_corr = tilt_deg - tilt_bias

            tilted = abs(tilt_corr) > TILT_THRESH_DEG
            status_text = "TILTED" if tilted else "LEVEL"
            status_color = (0, 0, 255) if tilted else (0, 255, 0)

            info_lines.insert(0, f"IDs: {ids_flat} (using: {chosen_id})")
            info_lines.append(f"tilt_raw = {tilt_deg:.2f} deg")
            info_lines.append(f"tilt_corr= {tilt_corr:.2f} deg (thr={TILT_THRESH_DEG:.1f})")
            info_lines.append(f"bias = {('None' if tilt_bias is None else f'{tilt_bias:.2f} deg')}")

        # 상태 크게 표시
        cv2.putText(frame, status_text, (10, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 1.6, (0, 0, 0), 6)
        cv2.putText(frame, status_text, (10, 60),
                    cv2.FONT_HERSHEY_SIMPLEX, 1.6, status_color, 3)

        # 정보 표시
        y = 95
        for line in info_lines:
            cv2.putText(frame, line, (10, y),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 4)
            cv2.putText(frame, line, (10, y),
                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            y += 28

        cv2.imshow("Tello ArUco Absolute Tilt", frame)
        key = cv2.waitKey(1) & 0xFF

        if key == 27:  # ESC
            break
        elif key == ord('r'):
            tilt_bias = None
            print("Bias reset.")
        elif key == ord('b'):
            # 마커가 잡힌 상태에서만 bias 설정
            if chosen_rvec is not None:
                tilt_bias = compute_tilt_deg_from_rvec(chosen_rvec)
                print(f"Bias set to {tilt_bias:.2f} deg (assumed LEVEL now).")
            else:
                print("No marker detected: cannot set bias.")

    tello.streamoff()
    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()