|
|
|
|
@ -46,7 +46,7 @@ def curses_main(stdscr):
|
|
|
|
|
stdscr.clear()
|
|
|
|
|
h, w = stdscr.getmaxyx()
|
|
|
|
|
|
|
|
|
|
stdscr.addstr(1, 2, "🛫 模式選單(使用 ↑↓ 選擇,Enter 發送,q 離開)")
|
|
|
|
|
stdscr.addstr(1, 2, "模式選單(使用 ↑↓ 選擇,Enter 發送,q 離開)")
|
|
|
|
|
|
|
|
|
|
for i, (name, _) in enumerate(mode_list):
|
|
|
|
|
if i == selected:
|
|
|
|
|
@ -77,19 +77,19 @@ def curses_main(stdscr):
|
|
|
|
|
param2=custom_mode,
|
|
|
|
|
param3=0, param4=0, param5=0, param6=0, param7=0
|
|
|
|
|
)
|
|
|
|
|
print("🧪 msg =", msg) # 確認封包物件生成
|
|
|
|
|
print("msg =", msg) # 確認封包物件生成
|
|
|
|
|
|
|
|
|
|
mav.send(msg) # ✅ 改為 send() 會寫入 capture
|
|
|
|
|
print("📦 RAW HEX:", capture.data.hex())
|
|
|
|
|
mav.send(msg) # 改為 send() 會寫入 capture
|
|
|
|
|
print("RAW HEX:", capture.data.hex())
|
|
|
|
|
|
|
|
|
|
api_frame = build_api_tx_frame(capture.data)
|
|
|
|
|
ser.write(api_frame)
|
|
|
|
|
|
|
|
|
|
# 顯示封包資訊
|
|
|
|
|
msg_line = min(h - 4, len(mode_list) + 5)
|
|
|
|
|
stdscr.addstr(msg_line, 2, f"✅ 發送模式切換:{name} ({custom_mode})")
|
|
|
|
|
stdscr.addstr(msg_line + 1, 2, f"📦 MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}")
|
|
|
|
|
stdscr.addstr(msg_line + 2, 2, f"📡 XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}")
|
|
|
|
|
stdscr.addstr(msg_line, 2, f"發送模式切換:{name} ({custom_mode})")
|
|
|
|
|
stdscr.addstr(msg_line + 1, 2, f"MAVLink HEX: {' '.join(f'{b:02x}' for b in capture.data)[:w-4]}")
|
|
|
|
|
stdscr.addstr(msg_line + 2, 2, f"XBee API HEX: {' '.join(f'{b:02x}' for b in api_frame)[:w-4]}")
|
|
|
|
|
stdscr.refresh()
|
|
|
|
|
curses.napms(1500)
|
|
|
|
|
|
|
|
|
|
@ -103,7 +103,7 @@ try:
|
|
|
|
|
curses.wrapper(lambda stdscr: curses_main(stdscr)) # 使用 lambda 函數來傳遞 ser
|
|
|
|
|
|
|
|
|
|
except Exception as e:
|
|
|
|
|
print(f"❌ 發生錯誤: {e}")
|
|
|
|
|
print(f"發生錯誤: {e}")
|
|
|
|
|
finally:
|
|
|
|
|
ser.close()
|
|
|
|
|
print("👋 程式結束,串口已關閉")
|
|
|
|
|
print("程式結束,串口已關閉")
|
|
|
|
|
|