|
|
import serial
|
|
|
import json
|
|
|
import threading
|
|
|
import time
|
|
|
from pynput import keyboard
|
|
|
|
|
|
SYSTEM_ID = 2
|
|
|
|
|
|
try:
|
|
|
ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1)
|
|
|
print("XBee 地面站啟動")
|
|
|
print("[1] START [2] STOP [3] GOTO [Esc] 退出程式")
|
|
|
except Exception as e:
|
|
|
print(f"無法開啟序列埠: {e}")
|
|
|
exit()
|
|
|
|
|
|
GPS_TYPE_LABELS = {
|
|
|
0: "no GPS",
|
|
|
1: "no fix",
|
|
|
2: "2D",
|
|
|
3: "3D",
|
|
|
4: "DGPS",
|
|
|
5: "RTK float",
|
|
|
6: "RTK fixed",
|
|
|
}
|
|
|
|
|
|
def format_uav_data(uav_data):
|
|
|
system_id = uav_data.get("s", "?")
|
|
|
mode = uav_data.get("m", "?")
|
|
|
battery = uav_data.get("b", "?")
|
|
|
position = uav_data.get("p", "?")
|
|
|
|
|
|
parts = [
|
|
|
f"[UAV] ID:{system_id}",
|
|
|
f"Mode:{mode}",
|
|
|
f"Bat:{battery}%",
|
|
|
f"Pos:{position}",
|
|
|
]
|
|
|
|
|
|
if "a" in uav_data:
|
|
|
armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED"
|
|
|
parts.append(f"Arm:{armed_text}")
|
|
|
|
|
|
ypr = uav_data.get("ypr")
|
|
|
if isinstance(ypr, list) and len(ypr) >= 3:
|
|
|
parts.append(f"Height:{uav_data.get('h', '?')}m")
|
|
|
parts.append(f"Yaw:{ypr[0]}deg")
|
|
|
parts.append(f"Pitch:{ypr[1]}deg")
|
|
|
parts.append(f"Roll:{ypr[2]}deg")
|
|
|
elif "y" in uav_data:
|
|
|
parts.append(f"Height:{uav_data.get('h', '?')}m")
|
|
|
parts.append(f"Yaw:{uav_data.get('y')}deg")
|
|
|
elif "h" in uav_data:
|
|
|
parts.append(f"Heading:{uav_data.get('h')}")
|
|
|
|
|
|
if "v" in uav_data:
|
|
|
parts.append(f"Vel:{uav_data.get('v')}m/s")
|
|
|
|
|
|
if "g" in uav_data:
|
|
|
gps_type = uav_data.get("g")
|
|
|
parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})")
|
|
|
|
|
|
dop = uav_data.get("d")
|
|
|
if isinstance(dop, list) and len(dop) >= 2:
|
|
|
parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}")
|
|
|
|
|
|
return " | ".join(parts)
|
|
|
|
|
|
def receive_thread():
|
|
|
while True:
|
|
|
try:
|
|
|
if ser.in_waiting > 0:
|
|
|
line = ser.readline().decode('utf-8', errors='ignore').strip()
|
|
|
if line:
|
|
|
try:
|
|
|
data = json.loads(line)
|
|
|
# 判斷是確認訊息（ack）還是 UAV 狀態
|
|
|
if "result" in data and "command" in data and "message" in data:
|
|
|
# 這是確認訊息
|
|
|
result = data.get("result")
|
|
|
cmd = data.get("command")
|
|
|
msg = data.get("message")
|
|
|
print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True)
|
|
|
else:
|
|
|
# 這是 UAV 狀態訊息
|
|
|
print(format_uav_data(data), flush=True)
|
|
|
except json.JSONDecodeError:
|
|
|
pass
|
|
|
except Exception as e:
|
|
|
print(f"\n[接收錯誤] {e}")
|
|
|
time.sleep(0.01)
|
|
|
|
|
|
t = threading.Thread(target=receive_thread, daemon=True)
|
|
|
t.start()
|
|
|
|
|
|
def send_command(command, position=None):
|
|
|
try:
|
|
|
if command not in {"START", "STOP", "GOTO"}:
|
|
|
raise ValueError("指令必須為大寫 START、STOP 或 GOTO")
|
|
|
cmd = {
|
|
|
"s": SYSTEM_ID,
|
|
|
"c": command
|
|
|
}
|
|
|
if position is not None:
|
|
|
cmd["p"] = position
|
|
|
|
|
|
payload = json.dumps(cmd, separators=(',', ':')) + "\n"
|
|
|
ser.write(payload.encode('utf-8'))
|
|
|
print(f"\n>> [指令已發送] {payload.strip()}")
|
|
|
except Exception as e:
|
|
|
print(f"\n發送失敗: {e}")
|
|
|
|
|
|
def on_press(key):
|
|
|
try:
|
|
|
if key.char == "1":
|
|
|
send_command("START")
|
|
|
elif key.char == "2":
|
|
|
send_command("STOP")
|
|
|
elif key.char == "3":
|
|
|
send_command("GOTO", [24.1199660, 120.677230])
|
|
|
except AttributeError:
|
|
|
if key == keyboard.Key.esc:
|
|
|
print("\n程式結束")
|
|
|
return False
|
|
|
|
|
|
with keyboard.Listener(on_press=on_press) as listener:
|
|
|
listener.join()
|