2.8.0 xbee and ws command
parent
1db3445e86
commit
ebd219c88e
@ -0,0 +1,179 @@
|
||||
import asyncio
|
||||
import websockets
|
||||
import json
|
||||
from urllib.parse import urlparse
|
||||
|
||||
# 可切換為 "essential" 或 "full"；預設廣播完整狀態。
|
||||
STATUS_FORMAT = "full"
|
||||
SYSTEM_ID = 2
|
||||
|
||||
# WebSocket 伺服器位址；修改此參數即可更換監聽 IP/port。
|
||||
WS_URL = "ws://0.0.0.0:8765"
|
||||
|
||||
|
||||
def parse_ws_url(ws_url):
|
||||
"""將 ws://host:port 解析為 websockets.serve 所需參數。"""
|
||||
parsed = urlparse(ws_url)
|
||||
if parsed.scheme != "ws":
|
||||
raise ValueError("WS_URL 必須使用 ws:// 格式")
|
||||
if not parsed.hostname:
|
||||
raise ValueError("WS_URL 缺少主機位址")
|
||||
try:
|
||||
port = parsed.port
|
||||
except ValueError as e:
|
||||
raise ValueError(f"WS_URL port 格式錯誤: {e}") from e
|
||||
if port is None:
|
||||
raise ValueError("WS_URL 缺少 port")
|
||||
return parsed.hostname, port
|
||||
|
||||
# 啟動伺服器
|
||||
async def main(ws_url=WS_URL):
|
||||
host, port = parse_ws_url(ws_url)
|
||||
async with websockets.serve(client_handler, host, port):
|
||||
print(f"WebSocket server started at {ws_url}")
|
||||
await asyncio.Future() # run forever
|
||||
|
||||
# 模擬資料
|
||||
def get_uav_status(status_format=None):
|
||||
status_format = status_format or STATUS_FORMAT
|
||||
essential_status = {
|
||||
"s": SYSTEM_ID, # system ID
|
||||
"m": "GUIDED", # mode
|
||||
"b": 78, # battery (%)
|
||||
"p": [24.1199660, 120.677230], # [lat, lon] (deg)
|
||||
"y": 178.5, # yaw (deg)
|
||||
}
|
||||
|
||||
full_status = {
|
||||
"s": SYSTEM_ID, # system ID
|
||||
"m": "GUIDED", # mode
|
||||
"a": 1, # 1=armed, 0=disarmed
|
||||
"b": 78, # battery (%)
|
||||
"h": 12.3, # height (m)
|
||||
"v": 4.2, # velocity (m/s)
|
||||
"p": [24.1199660, 120.677230], # [lat, lon] (deg)
|
||||
"ypr": [178.5, -0.1, 0.0], # yaw, pitch, roll (deg)
|
||||
"g": 3, # GPS fix type
|
||||
"d": [0.8, 1.2], # HDOP, VDOP
|
||||
}
|
||||
|
||||
if status_format == "essential":
|
||||
return essential_status
|
||||
if status_format == "full":
|
||||
return full_status
|
||||
raise ValueError(
|
||||
f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full")
|
||||
|
||||
# 處理 client 傳來的訊息
|
||||
async def handle_client_messages(websocket):
|
||||
try:
|
||||
async for message in websocket:
|
||||
try:
|
||||
data = json.loads(message)
|
||||
print(f"Received from client: {data}")
|
||||
system_id = data.get("s")
|
||||
cmd = data.get("c")
|
||||
# XBee 命令格式:
|
||||
# {"s":2,"c":"START"}
|
||||
# {"s":2,"c":"STOP"}
|
||||
# {"s":2,"c":"GOTO","p":[lat,lon]}
|
||||
if system_id is None:
|
||||
ack = {
|
||||
"result": "ERROR",
|
||||
"command": cmd,
|
||||
"message": "缺少 system_id 欄位 s",
|
||||
}
|
||||
await websocket.send(json.dumps(ack, ensure_ascii=False))
|
||||
print(f"Sent ack: {ack}")
|
||||
elif (isinstance(system_id, bool)
|
||||
or not isinstance(system_id, int)
|
||||
or system_id != SYSTEM_ID):
|
||||
ack = {
|
||||
"result": "ERROR",
|
||||
"command": cmd,
|
||||
"message": (
|
||||
f"指令目標 system_id={system_id} 不符，"
|
||||
f"本 UAV system_id={SYSTEM_ID}"
|
||||
),
|
||||
}
|
||||
await websocket.send(json.dumps(ack, ensure_ascii=False))
|
||||
print(f"Sent ack: {ack}")
|
||||
elif isinstance(cmd, str) and cmd in {"START", "STOP", "GOTO"}:
|
||||
if cmd == "START":
|
||||
print(f"收到 START 指令: system_id={system_id}")
|
||||
ack = {
|
||||
"result": "SUCCESS",
|
||||
"command": cmd,
|
||||
"message": "已接收 START 指令",
|
||||
}
|
||||
elif cmd == "STOP":
|
||||
print(f"收到 STOP 指令: system_id={system_id}")
|
||||
ack = {
|
||||
"result": "SUCCESS",
|
||||
"command": cmd,
|
||||
"message": "已接收 STOP 指令",
|
||||
}
|
||||
elif cmd == "GOTO":
|
||||
position = data.get("p")
|
||||
if isinstance(position, (list, tuple)) and len(position) >= 2:
|
||||
lat, lon = position[0], position[1]
|
||||
print(
|
||||
f"收到 GOTO 指令: system_id={system_id}, "
|
||||
f"lat={lat}, lon={lon}"
|
||||
)
|
||||
ack = {
|
||||
"result": "SUCCESS",
|
||||
"command": cmd,
|
||||
"message": "已接收 GOTO 指令",
|
||||
}
|
||||
else:
|
||||
print("GOTO 指令缺少位置 p=[lat,lon]")
|
||||
ack = {
|
||||
"result": "ERROR",
|
||||
"command": cmd,
|
||||
"message": "GOTO 指令缺少位置 p=[lat,lon]",
|
||||
}
|
||||
|
||||
await websocket.send(json.dumps(ack, ensure_ascii=False))
|
||||
print(f"Sent ack: {ack}")
|
||||
else:
|
||||
ack = {
|
||||
"result": "ERROR",
|
||||
"command": cmd,
|
||||
"message": "指令 c 必須為大寫 START、STOP 或 GOTO",
|
||||
}
|
||||
await websocket.send(json.dumps(ack, ensure_ascii=False))
|
||||
print(f"Sent ack: {ack}")
|
||||
except json.JSONDecodeError:
|
||||
print("❌ JSON 格式錯誤")
|
||||
except websockets.ConnectionClosed:
|
||||
print("❌ Client connection closed while receiving")
|
||||
|
||||
# 持續廣播 UAV 資訊
|
||||
async def broadcast_uav_status(websocket):
|
||||
try:
|
||||
while True:
|
||||
msg = get_uav_status()
|
||||
await websocket.send(json.dumps(msg))
|
||||
await asyncio.sleep(1) # 每秒發送一次
|
||||
except websockets.ConnectionClosed:
|
||||
print("❌ Client connection closed while sending")
|
||||
|
||||
# 每個 client 的處理流程
|
||||
async def client_handler(websocket, path=None):
|
||||
print(f"🔗 Client connected: {websocket.remote_address}")
|
||||
receive_task = asyncio.create_task(handle_client_messages(websocket))
|
||||
send_task = asyncio.create_task(broadcast_uav_status(websocket))
|
||||
|
||||
done, pending = await asyncio.wait(
|
||||
[receive_task, send_task],
|
||||
return_when=asyncio.FIRST_COMPLETED,
|
||||
)
|
||||
|
||||
for task in pending:
|
||||
task.cancel()
|
||||
|
||||
print(f"🔌 Client disconnected: {websocket.remote_address}")
|
||||
|
||||
if __name__ == "__main__":
|
||||
asyncio.run(main(WS_URL))
|
||||
Loading…
Reference in New Issue