pyaidrone 2.0__tar.gz → 2.1__tar.gz

{pyaidrone-2.0 → pyaidrone-2.1}/PKG-INFO

@@ -1,6 +1,6 @@
-Metadata-Version: 2.1
+Metadata-Version: 2.4
 Name: pyaidrone
-Version: 2.0
+Version: 2.1
 Summary: Library for AIDrone Products
 Home-page: http://www.ir-brain.com
 Author: IR-Brain
@@ -10,6 +10,17 @@ Classifier: License :: OSI Approved :: MIT License
 Classifier: Operating System :: OS Independent
 Requires-Python: >=3.6
 Description-Content-Type: text/markdown
+Requires-Dist: pyserial>=3.4
+Requires-Dist: pynput>=1.7.3
+Dynamic: author
+Dynamic: author-email
+Dynamic: classifier
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: home-page
+Dynamic: requires-dist
+Dynamic: requires-python
+Dynamic: summary
 
 ## install pyaidrone
 

pyaidrone-2.1/pyaidrone/aidrone_mavlink_bridge.py

@@ -0,0 +1,397 @@
+"""
+AIDrone ↔ MAVLink Bridge (GCS/QGroundControl 연동용)
+
+요구사항:
+  pip install pymavlink pyserial
+
+사용 예:
+  python aidrone_mavlink_bridge.py --port /dev/ttyACM0 --mav-in udp:0.0.0.0:14550 --mav-out udp:127.0.0.1:14551
+
+설명:
+  - MAVLink 입력(명령): COMMAND_LONG(ARM/TAKEOFF/LAND), MANUAL_CONTROL(조이스틱), HEARTBEAT
+  - MAVLink 출력(텔레메트리): HEARTBEAT, SYS_STATUS(배터리), ATTITUDE(roll/pitch/yaw), LOCAL_POSITION_NED(간단 위치), DISTANCE_SENSOR(ToF)
+"""
+
+import argparse
+import math
+import time
+import threading
+from typing import Optional
+
+from pymavlink import mavutil
+
+# pyaidrone 기반
+from pyaidrone.aiDrone import AIDrone  # takeoff/landing/velocity/rotation/altitude/Open/Close :contentReference[oaicite:3]{index=3}
+from pyaidrone.sensor import AIDroneSensor  # 수신 패킷을 state로 파싱 :contentReference[oaicite:4]{index=4}
+from pyaidrone.deflib import FRONT, BACK, RIGHT, LEFT  # 방향 상수 :contentReference[oaicite:5]{index=5}
+
+
+def clamp(v, lo, hi):
+    return lo if v < lo else hi if v > hi else v
+
+
+class AIDroneMavlinkBridge:
+    """
+    pyaidrone(AIDrone)와 MAVLink(GCS)를 연결하는 브릿지.
+    - 드론 시리얼은 이 프로세스만 단독으로 엽니다.
+    - MAVLink는 UDP(권장)로 GCS와 연결합니다.
+    """
+
+    def __init__(
+        self,
+        serial_port: str,
+        mav_in: str,
+        mav_out: str,
+        sysid: int = 1,
+        compid: int = 1,
+        telemetry_hz: float = 10.0,
+    ):
+        self.serial_port = serial_port
+        self.sysid = sysid
+        self.compid = compid
+
+        # AIDrone + 센서 파서
+        self.sensor = AIDroneSensor()
+        self.last_state = self.sensor.state.copy()
+        self.last_state_ts = 0.0
+
+        self.drone = AIDrone(receiveCallback=self._on_drone_packet)
+
+        # MAVLink I/O
+        self.mav_in = mavutil.mavlink_connection(mav_in, source_system=sysid, source_component=compid)
+        self.mav_out = mavutil.mavlink_connection(mav_out, source_system=sysid, source_component=compid)
+
+        self._stop = False
+        self._armed = False  # AIDrone가 명시적 arm API가 없어서 내부 상태로만 관리
+        self._last_hb = 0.0
+        self._telemetry_period = 1.0 / max(1.0, telemetry_hz)
+
+        # MANUAL_CONTROL 유실 시 자동 정지(안전)
+        self._last_manual_ts = 0.0
+        self.manual_timeout_s = 0.7  # 이 시간 동안 조이스틱 명령이 없으면 정지
+
+        # yaw 누적(센서에 yaw가 없으면 “추정치”로 MAVLink에만 표시)
+        self._yaw_est = 0.0
+        self._yaw_rate_last = 0.0
+        self._yaw_est_ts = time.time()
+
+    # ---------------------------
+    # Drone callback
+    # ---------------------------
+    def _on_drone_packet(self, packet):
+        st = self.sensor.parse(packet)
+        if st:
+            self.last_state = st
+            self.last_state_ts = time.time()
+
+    # ---------------------------
+    # Lifecycle
+    # ---------------------------
+    def start(self):
+        ok = self.drone.Open(self.serial_port)
+        if not ok:
+            raise RuntimeError("AIDrone serial open failed")
+
+        t_rx = threading.Thread(target=self._rx_loop, daemon=True)
+        t_tx = threading.Thread(target=self._tx_loop, daemon=True)
+        t_safe = threading.Thread(target=self._safety_loop, daemon=True)
+
+        t_rx.start()
+        t_tx.start()
+        t_safe.start()
+
+        print("[Bridge] running. Ctrl+C to stop.")
+        try:
+            while True:
+                time.sleep(0.2)
+        except KeyboardInterrupt:
+            self._stop = True
+            time.sleep(0.3)
+        finally:
+            try:
+                self.drone.Close()
+            except Exception:
+                pass
+
+    # ---------------------------
+    # MAVLink RX (GCS -> Drone)
+    # ---------------------------
+    def _rx_loop(self):
+        while not self._stop:
+            msg = self.mav_in.recv_match(blocking=True, timeout=0.5)
+            if msg is None:
+                continue
+
+            mtype = msg.get_type()
+            if mtype == "BAD_DATA":
+                continue
+
+            if mtype == "COMMAND_LONG":
+                self._handle_command_long(msg)
+            elif mtype == "MANUAL_CONTROL":
+                self._handle_manual_control(msg)
+            elif mtype == "HEARTBEAT":
+                # 필요시 링크 감지용으로만 사용
+                pass
+
+    def _ack_command(self, command: int, result: int):
+        self.mav_out.mav.command_ack_send(command, result, 0, 0, 0, 0)
+
+    def _handle_command_long(self, msg):
+        cmd = int(msg.command)
+
+        # ARM/DISARM: 실제 AIDrone에 arm 개념이 없으니 내부 플래그만 관리
+        if cmd == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+            arm = (msg.param1 >= 0.5)
+            self._armed = bool(arm)
+            self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED)
+            return
+
+        # TAKEOFF
+        if cmd == mavutil.mavlink.MAV_CMD_NAV_TAKEOFF:
+            try:
+                self.drone.takeoff()
+                self._armed = True
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED)
+            except Exception:
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_FAILED)
+            return
+
+        # LAND
+        if cmd == mavutil.mavlink.MAV_CMD_NAV_LAND:
+            try:
+                self.drone.landing()
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED)
+            except Exception:
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_FAILED)
+            return
+
+        # EMERGENCY STOP (지원하고 싶으면 MAV_CMD_DO_FLIGHTTERMINATION 등으로 매핑 가능)
+        if cmd == mavutil.mavlink.MAV_CMD_DO_FLIGHTTERMINATION:
+            try:
+                self.drone.emergency()
+                self._armed = False
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_ACCEPTED)
+            except Exception:
+                self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_FAILED)
+            return
+
+        self._ack_command(cmd, mavutil.mavlink.MAV_RESULT_UNSUPPORTED)
+
+    def _handle_manual_control(self, msg):
+        """
+        MANUAL_CONTROL: 조이스틱 입력
+          x: -1000..1000 (pitch: 전/후)
+          y: -1000..1000 (roll: 좌/우)
+          z: 0..1000 (throttle)
+          r: -1000..1000 (yaw)
+        """
+        self._last_manual_ts = time.time()
+
+        # arming 안됐으면 제어 무시(안전)
+        if not self._armed:
+            return
+
+        x = int(msg.x)   # forward/back
+        y = int(msg.y)   # left/right
+        z = int(msg.z)   # throttle
+        r = int(msg.r)   # yaw
+
+        # 1) 전/후, 좌/우는 AIDrone.velocity(dir, vel 0~100)로 매핑
+        #    AIDrone.velocity는 dir에 따라 부호를 내부에서 처리함(앞/뒤, 우/좌). :contentReference[oaicite:6]{index=6}
+        forward = clamp(x / 1000.0, -1.0, 1.0)
+        lateral = clamp(y / 1000.0, -1.0, 1.0)
+
+        # deadzone
+        dead = 0.12
+        if abs(forward) < dead:
+            forward = 0.0
+        if abs(lateral) < dead:
+            lateral = 0.0
+
+        vel_f = int(abs(forward) * 100)
+        vel_l = int(abs(lateral) * 100)
+
+        # 전/후
+        if vel_f > 0:
+            self.drone.velocity(FRONT if forward > 0 else BACK, vel_f)
+
+        # 좌/우: pyaidrone에서 RIGHT=2, LEFT=3 :contentReference[oaicite:7]{index=7}
+        if vel_l > 0:
+            self.drone.velocity(RIGHT if lateral > 0 else LEFT, vel_l)
+
+        # 둘 다 0이면 정지(velocity에 0 넣기)
+        if vel_f == 0 and vel_l == 0:
+            self.drone.velocity(FRONT, 0)
+
+        # 2) yaw: rotation(rot)으로 매핑 (상대 회전)
+        yaw_in = clamp(r / 1000.0, -1.0, 1.0)
+        if abs(yaw_in) >= 0.15:
+            # 입력 크기에 따라 작은 각도로 “자주” 회전
+            step_deg = int(yaw_in * 8)  # 1회에 최대 ±8도
+            if step_deg != 0:
+                self.drone.rotation(step_deg)
+                self._yaw_rate_last = yaw_in
+
+        # 3) throttle: altitude(alt)로 매핑
+        #    pyaidrone의 takeoff는 alt=70을 사용(단위는 cm로 보임) :contentReference[oaicite:8]{index=8}
+        #    z는 0..1000 이므로 50~150cm로 스케일(원하는 범위로 조정 가능)
+        alt_cm = int(50 + (clamp(z / 1000.0, 0.0, 1.0) * 100))  # 50~150
+        try:
+            self.drone.altitude(alt_cm)
+        except Exception:
+            pass
+
+    # ---------------------------
+    # Safety: MANUAL 끊기면 정지
+    # ---------------------------
+    def _safety_loop(self):
+        while not self._stop:
+            if self._armed and (time.time() - self._last_manual_ts) > self.manual_timeout_s:
+                try:
+                    # 정지
+                    self.drone.velocity(FRONT, 0)
+                except Exception:
+                    pass
+            time.sleep(0.1)
+
+    # ---------------------------
+    # MAVLink TX (Drone -> GCS)
+    # ---------------------------
+    def _tx_loop(self):
+        while not self._stop:
+            now = time.time()
+
+            # yaw 추정 업데이트(센서 yaw가 없으므로 r 입력 기반 아주 단순 적분)
+            dt = max(0.0, now - self._yaw_est_ts)
+            self._yaw_est_ts = now
+            # yaw_rate_last는 -1..1, 초당 약 30deg로 가정(표시용)
+            self._yaw_est += math.radians(30.0) * float(self._yaw_rate_last) * dt
+            # -pi..pi로 정규화
+            self._yaw_est = (self._yaw_est + math.pi) % (2 * math.pi) - math.pi
+            self._yaw_rate_last *= 0.85  # 서서히 감쇠
+
+            # 1Hz heartbeat
+            if now - self._last_hb >= 1.0:
+                self._send_heartbeat()
+                self._last_hb = now
+
+            # telemetry
+            st = self.last_state
+
+            self._send_sys_status(st)
+            self._send_attitude(st)
+            self._send_local_position(st)
+            self._send_distance_sensor(st)
+
+            time.sleep(self._telemetry_period)
+
+    def _send_heartbeat(self):
+        base_mode = 0
+        if self._armed:
+            base_mode |= mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED
+
+        self.mav_out.mav.heartbeat_send(
+            mavutil.mavlink.MAV_TYPE_QUADROTOR,
+            mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+            base_mode,
+            0,  # custom_mode
+            mavutil.mavlink.MAV_STATE_ACTIVE
+        )
+
+    def _send_sys_status(self, st: dict):
+        # battery: pyaidrone sensor는 battery(%)를 제공 :contentReference[oaicite:9]{index=9}
+        batt_rem = int(st.get("battery", 0))
+        batt_rem = clamp(batt_rem, 0, 100)
+
+        # 전압은 센서에서 직접 안 주므로 -1(unknown). 남은%만 전달.
+        self.mav_out.mav.sys_status_send(
+            0, 0, 0,          # sensors present/enabled/health
+            0,                # load
+            0,                # voltage_battery(mV) unknown
+            -1,               # current_battery(cA) unknown
+            batt_rem,
+            0, 0, 0, 0, 0, 0
+        )
+
+    def _send_attitude(self, st: dict):
+        # sensor.py: roll/pitch가 short로 들어오며 단위는 보드 정의(대개 degree 또는 0.1deg일 가능성)
+        # 여기서는 “degree”라고 가정하고 rad로 변환(필요시 스케일 조정)
+        roll_raw = float(st.get("roll", 0))
+        pitch_raw = float(st.get("pitch", 0))
+
+        # 스케일 추정: 만약 값이 너무 크면(예: 300~500) 0.1deg일 가능성 → 0.1 배
+        if abs(roll_raw) > 180 or abs(pitch_raw) > 180:
+            roll_raw *= 0.1
+            pitch_raw *= 0.1
+
+        roll = math.radians(roll_raw)
+        pitch = math.radians(pitch_raw)
+        yaw = float(self._yaw_est)
+
+        self.mav_out.mav.attitude_send(
+            int(time.time() * 1000),
+            roll, pitch, yaw,
+            0.0, 0.0, 0.0
+        )
+
+    def _send_local_position(self, st: dict):
+        # sensor.py: pos_x/pos_y(cm), height(cm) 제공 :contentReference[oaicite:10]{index=10}
+        px_cm = float(st.get("pos_x", 0))
+        py_cm = float(st.get("pos_y", 0))
+        h_cm = float(st.get("height", 0))
+
+        x = px_cm / 100.0
+        y = py_cm / 100.0
+        z = -(h_cm / 100.0)  # NED에서 z는 Down(+). 높이는 Up이므로 음수
+
+        # 속도는 센서에 없으니 0
+        self.mav_out.mav.local_position_ned_send(
+            int(time.time() * 1000),
+            x, y, z,
+            0.0, 0.0, 0.0
+        )
+
+    def _send_distance_sensor(self, st: dict):
+        # tof(mm) 제공 :contentReference[oaicite:11]{index=11}
+        tof_mm = int(st.get("tof", 0))
+        if tof_mm <= 0:
+            return
+
+        # MAVLink DISTANCE_SENSOR: cm 단위로 쓰는 경우도 있지만 메시지는 mm 기반(min/max는 cm 단위 필드 아님)
+        # 여기서는 current_distance를 mm로 그대로 넣고, min/max는 적당히 설정
+        self.mav_out.mav.distance_sensor_send(
+            int(time.time() * 1000),
+            50,      # min_distance (cm) - 센서 최소
+            2000,    # max_distance (cm) - 센서 최대(대략)
+            tof_mm,  # current_distance (mm)
+            mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER,
+            0,       # id
+            mavutil.mavlink.MAV_SENSOR_ROTATION_PITCH_270,  # 아래 방향
+            0        # covariance
+        )
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--port", required=True, help="AIDrone serial port (e.g., COM3 or /dev/ttyACM0)")
+    ap.add_argument("--mav-in", default="udp:0.0.0.0:14550", help="MAVLink input (GCS -> bridge)")
+    ap.add_argument("--mav-out", default="udp:127.0.0.1:14551", help="MAVLink output (bridge -> GCS)")
+    ap.add_argument("--sysid", type=int, default=1)
+    ap.add_argument("--compid", type=int, default=1)
+    ap.add_argument("--hz", type=float, default=10.0)
+    args = ap.parse_args()
+
+    bridge = AIDroneMavlinkBridge(
+        serial_port=args.port,
+        mav_in=args.mav_in,
+        mav_out=args.mav_out,
+        sysid=args.sysid,
+        compid=args.compid,
+        telemetry_hz=args.hz,
+    )
+    bridge.start()
+
+
+if __name__ == "__main__":
+    main()

{pyaidrone-2.0 → pyaidrone-2.1}/pyaidrone.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
-Metadata-Version: 2.1
+Metadata-Version: 2.4
 Name: pyaidrone
-Version: 2.0
+Version: 2.1
 Summary: Library for AIDrone Products
 Home-page: http://www.ir-brain.com
 Author: IR-Brain
@@ -10,6 +10,17 @@ Classifier: License :: OSI Approved :: MIT License
 Classifier: Operating System :: OS Independent
 Requires-Python: >=3.6
 Description-Content-Type: text/markdown
+Requires-Dist: pyserial>=3.4
+Requires-Dist: pynput>=1.7.3
+Dynamic: author
+Dynamic: author-email
+Dynamic: classifier
+Dynamic: description
+Dynamic: description-content-type
+Dynamic: home-page
+Dynamic: requires-dist
+Dynamic: requires-python
+Dynamic: summary
 
 ## install pyaidrone
 

{pyaidrone-2.0 → pyaidrone-2.1}/pyaidrone.egg-info/SOURCES.txt

@@ -3,6 +3,7 @@ setup.cfg
 setup.py
 pyaidrone/__init__.py
 pyaidrone/aiDrone.py
+pyaidrone/aidrone_mavlink_bridge.py
 pyaidrone/deflib.py
 pyaidrone/edu.py
 pyaidrone/ikeyevent.py

{pyaidrone-2.0 → pyaidrone-2.1}/setup.py

@@ -2,7 +2,7 @@ from setuptools import setup, find_packages
 
 setup(
     name="pyaidrone",
-    version="2.0",
+    version="2.1",
     description="Library for AIDrone Products",
     long_description=open("README.md").read(),  # README.md 내용을 long_description으로 사용
     long_description_content_type="text/markdown",  # README 파일이 markdown 형식임을 지정