79 lines
2.6 KiB
Plaintext
79 lines
2.6 KiB
Plaintext
import serial
|
|
import time
|
|
|
|
# 확인된 포트 경로 설정
|
|
SERIAL_PORT = "/dev/cu.usbserial-0001"
|
|
BAUD_RATE = 420000
|
|
|
|
CRSF_SYNC = 0xC8
|
|
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16
|
|
|
|
def parse_channels(payload):
|
|
if len(payload) < 22:
|
|
return []
|
|
|
|
# 22바이트의 채널 데이터를 정수로 변환 후 11비트씩 언패킹
|
|
data = int.from_bytes(payload[:22], byteorder='little')
|
|
channels = []
|
|
for _ in range(16):
|
|
raw_val = data & 0x07FF # 11-bit 마스크
|
|
# CRSF 원본 값(172~1811)을 RC 마이크로초(1000~2000µs) 단위로 변환
|
|
us_val = int((raw_val - 172) * (2000 - 1000) / (1811 - 172) + 1000)
|
|
channels.append(us_val)
|
|
data >>= 11
|
|
return channels
|
|
|
|
def main():
|
|
try:
|
|
ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=0.1)
|
|
print(f"Connected to {SERIAL_PORT} at {BAUD_RATE} baud.")
|
|
print("스틱과 스위치를 움직여 채널 값을 확인하세요. (종료: Ctrl+C)\n")
|
|
except Exception as e:
|
|
print(f"포트 연결 오류: {e}")
|
|
return
|
|
|
|
buf = bytearray()
|
|
while True:
|
|
try:
|
|
if ser.in_waiting:
|
|
buf.extend(ser.read(ser.in_waiting))
|
|
|
|
while len(buf) > 3:
|
|
if buf[0] != CRSF_SYNC:
|
|
buf.pop(0)
|
|
continue
|
|
|
|
length = buf[1]
|
|
total_packet_len = length + 2
|
|
|
|
if len(buf) < total_packet_len:
|
|
break # 전체 패킷 대기
|
|
|
|
packet = buf[:total_packet_len]
|
|
buf = buf[total_packet_len:]
|
|
|
|
frame_type = packet[2]
|
|
if frame_type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
|
|
payload = packet[3:-1] # CRC 제외 payload
|
|
channels = parse_channels(payload)
|
|
|
|
# read_crsf.py 출력 부분 수정
|
|
print(
|
|
f"\rCH1: {channels[0]:4d} | CH2: {channels[1]:4d} | "
|
|
f"CH3: {channels[2]:4d} | CH4: {channels[3]:4d} | "
|
|
f"CH5(SA/SC): {channels[4]:4d} | CH6(SD): {channels[5]:4d} | "
|
|
f"CH7: {channels[6]:4d} | CH8: {channels[7]:4d}",
|
|
end="", flush=True
|
|
)
|
|
|
|
except KeyboardInterrupt:
|
|
print("\n모니터링 종료")
|
|
break
|
|
except Exception as e:
|
|
print(f"\n데이터 수신 오류: {e}")
|
|
break
|
|
|
|
ser.close()
|
|
|
|
if __name__ == "__main__":
|
|
main() |