crsf 姿态解析python样例代码

import struct
import threading
import time
from enum import IntEnum

import serial

class PacketsTypes(IntEnum):
    GPS = 0x02
    VARIO = 0x07
    BATTERY_SENSOR = 0x08
    BARO_ALT = 0x09
    HEARTBEAT = 0x0B
    VIDEO_TRANSMITTER = 0x0F
    LINK_STATISTICS = 0x14
    RC_CHANNELS_PACKED = 0x16
    ATTITUDE = 0x1E
    FLIGHT_MODE = 0x21
    DEVICE_INFO = 0x29
    CONFIG_READ = 0x2C
    CONFIG_WRITE = 0x2D
    RADIO_ID = 0x3A

class CrsfParser:
    """CRSF协议解析器"""
    SYNC_BYTE = 0xC8  # CRSF接收机地址
    MAX_PACKET_SIZE = 64
    VALID_ADDRESSES = {0xC8, 0xEA, 0xEC, 0xEE}

    def __init__(self):
        self.buffer = bytearray()
        self.callbacks = {}
        self.running = False
        self.serial_port = None
        self.receive_thread = None
        self.packet_count = 0
        self.error_count = 0
        self._lock = threading.Lock()  # 添加锁保护缓冲区

    def open_serial(self, port: str, baudrate: int = 416666) -> bool:
        """打开串口连接"""
        try:
            self.serial_port = serial.Serial(
                port=port,
                baudrate=baudrate,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=0.1,
                rtscts=False,
                dsrdtr=False
            )
            print(f"已打开串口 {port}, 波特率 {baudrate}")
            return True
        except serial.SerialException as e:
            print(f"打开串口失败: {e}")
            return False

    def close_serial(self):
        """关闭串口连接"""
        if self.serial_port and self.serial_port.is_open:
            self.serial_port.close()
            print("串口已关闭")

    def start_receive(self):
        """开始接收数据"""
        if self.serial_port is None or not self.serial_port.is_open:
            raise RuntimeError("串口未打开")

        self.running = True
        self.receive_thread = threading.Thread(target=self._receive_loop)
        self.receive_thread.daemon = True
        self.receive_thread.start()
        print("开始接收数据...")

    def stop_receive(self):
        """停止接收数据"""
        self.running = False
        if self.receive_thread:
            self.receive_thread.join(timeout=1.0)
        print("停止接收数据")

    def _receive_loop(self):
        """接收循环"""
        while self.running and self.serial_port and self.serial_port.is_open:
            try:
                # 读取可用数据
                if self.serial_port.in_waiting > 0:
                    frame = self.read_frame(self.serial_port)
                    if frame is None:
                        continue
                    self.feed_data(frame)
                else:
                    time.sleep(0.001)  # 短暂休眠
            except serial.SerialException as e:
                print(f"串口通信错误: {e}")
                time.sleep(0.1)
            except Exception as e:
                print(f"接收数据错误: {e}")
                time.sleep(0.1)

    def read_frame(self, ser):
        """读取一帧 CRSF 数据"""
        frame = bytearray()
        while True:
            b = ser.read(1)
            if len(b) == 0:
                return None
            if b[0] == 0xC8:
                break

        length_byte = ser.read(1)
        if len(length_byte) == 0:
            return None
        length = length_byte[0]
        payload_crc = ser.read(length)

        if len(payload_crc) != length:
            return None

        frame.extend(b)
        frame.extend(length_byte)
        frame.extend(payload_crc)

        return frame

    def feed_data(self, frame):
        ptype = frame[2]
        payload = frame[3:]
        if ptype == PacketsTypes.ATTITUDE:
            self.parse_attitude(payload)
        # else:
        #     packet = ' '.join(map(hex, frame))
        #     print(f"Unknown 0x{ptype:02x}: {packet}")

    def parse_attitude(self, payload):
        """解析姿态（pitch roll yaw）单位 0.1°"""
        roll = struct.unpack(">h", payload[0:2])[0] / 10000 * 180 / 3.1415
        pitch = struct.unpack(">h", payload[2:4])[0] / 10000 * 180 / 3.1415
        yaw = struct.unpack(">h", payload[4:6])[0] / 10000 * 180 / 3.1415

        self.attitude_data = {
            'pitch': pitch,
            'roll': roll,
            'yaw': ((yaw + 360) % 360)
        }
        print(f"姿态: 俯仰={self.attitude_data['pitch']:.2f}°  "
              f"横滚=Roll={self.attitude_data['roll']:.2f}°  "
              f"偏航={self.attitude_data['yaw']:.2f}°")


if __name__ == "__main__":
    crsf = CrsfParser()
    crsf.open_serial('COM5', baudrate=420000)
    crsf.start_receive()

    input()