Compare commits
| Author | SHA1 | Date | |
|---|---|---|---|
 ken910606
|
a2c1d23f3d |
@@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort
|
||||
from .utils.acquirePort import find_available_port
|
||||
|
||||
logger = setup_logger(os.path.basename(__file__))
|
||||
PROJECT_VER = "v1.25"
|
||||
PROJECT_VER = "v1.20"
|
||||
|
||||
class PanelState:
|
||||
def __init__(self):
|
||||
@@ -490,10 +490,6 @@ class ControlPanel:
|
||||
# 一樣退兩層
|
||||
menu_stack.pop()
|
||||
idx_stack.pop()
|
||||
|
||||
elif selected.action == "DO_DISCOVERY_PANEL":
|
||||
if hasattr(selected, 'serial_id'):
|
||||
cmd_q.put(("DO_DISCOVERY", selected.serial_id))
|
||||
|
||||
# related with -> 列表選單
|
||||
elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
|
||||
@@ -872,7 +868,6 @@ class ControlPanel:
|
||||
obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
|
||||
MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
|
||||
MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
|
||||
MenuNode("Discovery", "搜尋遠端RF(Only XBEE-ESP32)", "DO_DISCOVERY_PANEL"),
|
||||
# MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
|
||||
MenuNode("GoUp", "回到列表", "BACK"),
|
||||
])
|
||||
@@ -1324,11 +1319,6 @@ class Orchestrator:
|
||||
elif action == "REMOVE_LINKED_SERIAL":
|
||||
serial_id = cmd[1]
|
||||
self.plumber.remove_serial_link(serial_id)
|
||||
elif action == "DO_DISCOVERY":
|
||||
serial_id = cmd[1]
|
||||
processor = self.plumber.get_espv1_processor(serial_id)
|
||||
if processor:
|
||||
processor.request_discovery()
|
||||
|
||||
elif action == "INSPECT_VEHICLE":
|
||||
sysid, compid = cmd[1], cmd[2]
|
||||
@@ -1696,8 +1686,4 @@ if __name__ == "__main__":
|
||||
2025.05.28
|
||||
1. 工程模式 新增 Node 重啟的功能
|
||||
|
||||
2026.08.05 (ver 1.25)
|
||||
1. 增加對於 esp32 連線模式的支援
|
||||
|
||||
|
||||
'''
|
||||
|
||||
@@ -37,7 +37,6 @@ class ComponentType(Enum):
|
||||
class RFModuleType(Enum):
|
||||
"""RF模組類型"""
|
||||
XBEE = "xbee"
|
||||
XBEE_ESP = "xbee_esp"
|
||||
UDP = "udp"
|
||||
TCP = "tcp"
|
||||
OTHER = "other"
|
||||
@@ -182,7 +181,6 @@ class RFStatus:
|
||||
at_response: Optional[str] = None # AT 命令回應
|
||||
link_quality: Optional[int] = None # 連接品質
|
||||
timestamp: Optional[float] = None # 時間戳記
|
||||
avg_bandwidth: Optional[float] = None # 量測到的傳輸速度 bytes/ms
|
||||
custom_status: Dict[str, Any] = field(default_factory=dict) # 其他自定義狀態
|
||||
|
||||
|
||||
@@ -276,18 +274,6 @@ class RFModule:
|
||||
self.type = rf_type
|
||||
self.status = RFStatus()
|
||||
self.socket_info = SocketInfo()
|
||||
|
||||
# # only for ESP32
|
||||
# self.poll_data_times: deque[int] = None
|
||||
# self.poll_data_sizes: deque[int] = None
|
||||
# self.poll_total_time = None
|
||||
# self.poll_total_size = None
|
||||
# self.remain_bytes = None
|
||||
|
||||
def update_avg_bandwidth(self, bandwidth: float, timestamp: Optional[float] = None) -> None:
|
||||
self.status.avg_bandwidth = bandwidth
|
||||
if timestamp:
|
||||
self.status.timestamp = timestamp
|
||||
|
||||
def update_rssi(self, rssi: int, timestamp: Optional[float] = None) -> None:
|
||||
"""更新RSSI"""
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -7,54 +7,42 @@ POLL 輪詢策略，供 XBeeFrameProcessor_ESPv1 使用。
|
||||
from dataclasses import dataclass
|
||||
from typing import List
|
||||
|
||||
MIN_TRIGGER_INTERVAL_MS = 5
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PollDevice:
|
||||
"""從 esp32AddrMapping 抽出的唯讀快照。"""
|
||||
address_64: bytes
|
||||
# system_id: int
|
||||
remain_bytes: int
|
||||
last_poll_time: float
|
||||
last_done_time: float
|
||||
|
||||
|
||||
def pick_next(devices: List[PollDevice], now_time):
|
||||
@dataclass
|
||||
class PollSchedulerState:
|
||||
"""每條 serial link 一份，保存 round-robin 索引。"""
|
||||
round_robin_index: int = 0
|
||||
|
||||
|
||||
def pick_next(
|
||||
devices: List[PollDevice],
|
||||
scheduler_state: PollSchedulerState
|
||||
):
|
||||
"""
|
||||
選下一個 POLL 目標。
|
||||
|
||||
回傳 (target_address_64, grant_bytes)；devices 為空時回傳 (None, 0)。
|
||||
|
||||
poll time = 0.2305 * grant_bytes + 58.094 (R平方為0.979 高線性相關) @-40db
|
||||
"""
|
||||
if not devices:
|
||||
return None, 0
|
||||
|
||||
grant_bytes = 0
|
||||
device_count = len(devices)
|
||||
weights = [0] * device_count
|
||||
selected_index = scheduler_state.round_robin_index % device_count
|
||||
selected_device = devices[selected_index]
|
||||
scheduler_state.round_robin_index += 1
|
||||
|
||||
for i in range(device_count):
|
||||
gap = int((now_time - devices[i].last_poll_time)*1000)
|
||||
# # 時間大於觸發間隔 才會計算
|
||||
# if gap >= MIN_TRIGGER_INTERVAL_MS:
|
||||
# weights[i] += devices[i].remain_bytes
|
||||
# weights[i] += gap
|
||||
|
||||
weights[i] += devices[i].remain_bytes
|
||||
weights[i] += gap
|
||||
|
||||
maxarg = weights.index(max(weights))
|
||||
|
||||
if weights[maxarg] == 0:
|
||||
return None, 0
|
||||
|
||||
selected_device = devices[maxarg]
|
||||
|
||||
if selected_device.remain_bytes < 500:
|
||||
grant_bytes = 170 # 這個大概是 100 ms 總傳輸時間
|
||||
elif selected_device.remain_bytes < 1200:
|
||||
grant_bytes = 610 # 這個大概是 200 ms 總傳輸時間
|
||||
else:
|
||||
grant_bytes = 1050 # 這個大概是 300 ms 總傳輸時間
|
||||
grant_bytes = 0
|
||||
if selected_device.remain_bytes > 0:
|
||||
grant_bytes = min( 65535, max(0, selected_device.remain_bytes))
|
||||
|
||||
return selected_device.address_64, grant_bytes
|
||||
|
||||
|
||||
@@ -27,7 +27,6 @@
|
||||
python -m fc_network_adapter.tests.test_vehicleStatusPublisher
|
||||
python -m fc_network_adapter.tests.test_ringBuffer
|
||||
python -m fc_network_adapter.fc_network_adapter.mainOrchestrator
|
||||
python -m fc_network_adapter.fc_network_adapter.serialManager
|
||||
|
||||
python -m someotherpkg.src.example_takeoff_land
|
||||
python -m someotherpkg.src.example_change_mode
|
||||
|
||||
@@ -1,641 +0,0 @@
|
||||
from machine import UART
|
||||
import time
|
||||
import struct
|
||||
import gc
|
||||
|
||||
# =========================================================
|
||||
# ESP32 / MicroPython：UAV 端 XBee <-> Flight Controller Bridge
|
||||
# Packet-size TDMA + DONE + SYSID / GCS address 自動學習版
|
||||
#
|
||||
# 本版已刪除「一般 DISC / 強制 DSCF」雙模式。
|
||||
# 現在只保留一種 discovery 封包，名稱統一叫 DISC。
|
||||
#
|
||||
# 重要：
|
||||
# - DISC 的功能等同於原本的 DSCF，也就是「強制 discovery」。
|
||||
# - UAV 收到 DISC 後，只要已知 MY_SYSID 與 DEST_64，就會重新回 HELO。
|
||||
# - 若收到 DISC 時 MY_SYSID 尚未學到，會設定 DISC_PENDING；
|
||||
# 之後一旦從飛控 MAVLink 學到 SYSID，就自動補送 HELO。
|
||||
#
|
||||
# 封包格式：
|
||||
# GCS -> UAV:
|
||||
# DISC
|
||||
# POLL + target_sysid(1) + grant_bytes(2)
|
||||
#
|
||||
# UAV -> GCS:
|
||||
# HELO + sysid(1)
|
||||
# DONE + sysid(1) + sent_len(2) + remain_len(2)
|
||||
#
|
||||
# 注意：
|
||||
# - HELO 不帶 remain。
|
||||
# - DONE 保留 remain。
|
||||
# - MY_SYSID 不寫死，由飛控 MAVLink 自動學。
|
||||
# - DEST_64 不寫死，由 GCS 封包的 XBee 0x90 src64 自動學。
|
||||
# =========================================================
|
||||
|
||||
|
||||
# ================= 設定區 =================
|
||||
FC_BAUDRATE = 115200
|
||||
XB_BAUDRATE = 115200
|
||||
|
||||
DEST_64 = None
|
||||
MY_SYSID = None
|
||||
|
||||
SYSID_LEARN_CONFIRM_COUNT = 3
|
||||
_sysid_candidate = None
|
||||
_sysid_candidate_count = 0
|
||||
|
||||
# 若收到 DISC 時還沒學到 MY_SYSID，就先記住。
|
||||
# 等之後從 FC MAVLink 學到 MY_SYSID 後，自動補送 HELO。
|
||||
DISC_PENDING = False
|
||||
|
||||
XBEE_MAX_PAYLOAD = 100
|
||||
|
||||
# Discovery：只保留 DISC；功能等同原本 DSCF，收到後強制回 HELO。
|
||||
DISC_MAGIC = b'DISC'
|
||||
|
||||
# UAV -> GCS hello：
|
||||
# HELO + sysid(1)
|
||||
HELLO_MAGIC = b'HELO'
|
||||
|
||||
HELLO_MIN_INTERVAL_MS = 1000
|
||||
_last_hello_ms = 0
|
||||
|
||||
# 保留此變數只作狀態記錄；本版 DISC 會 force=True，所以不會被 HELLO_SENT 擋掉。
|
||||
HELLO_SENT = False
|
||||
|
||||
POLL_MAGIC = b'POLL'
|
||||
DEFAULT_GRANT_BYTES = 600
|
||||
|
||||
DONE_MAGIC = b'DONE'
|
||||
|
||||
MAX_BUF_SIZE = 6144
|
||||
READ_FC_BYTES = 250
|
||||
|
||||
FC_UART_ID = 1
|
||||
FC_TX_PIN = 32
|
||||
FC_RX_PIN = 33
|
||||
|
||||
XB_UART_ID = 2
|
||||
XB_TX_PIN = 25
|
||||
XB_RX_PIN = 26
|
||||
# =========================================
|
||||
|
||||
|
||||
uart_fc = UART(FC_UART_ID, baudrate=FC_BAUDRATE, tx=FC_TX_PIN, rx=FC_RX_PIN, rxbuf=4096)
|
||||
uart_xb = UART(XB_UART_ID, baudrate=XB_BAUDRATE, tx=XB_TX_PIN, rx=XB_RX_PIN, rxbuf=4096)
|
||||
|
||||
tx_buf = bytearray() # FC -> GCS 等待 TDMA poll 的 MAVLink stream
|
||||
rx_buf = bytearray() # XBee API frame parser buffer
|
||||
|
||||
|
||||
# =========================================================
|
||||
# 基本工具函式
|
||||
# =========================================================
|
||||
|
||||
def get_checksum(data):
|
||||
return 0xFF - (sum(data) & 0xFF)
|
||||
|
||||
|
||||
def is_valid_addr64(addr):
|
||||
if addr is None:
|
||||
return False
|
||||
if len(addr) != 8:
|
||||
return False
|
||||
if addr == b'\x00\x00\x00\x00\x00\x00\x00\x00':
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def learn_dest64_from_src64(src64):
|
||||
"""
|
||||
UAV 收到 GCS 的 XBee 0x90 frame 時，
|
||||
0x90 裡面的 src64 就是 GCS / coordinator XBee 的 64-bit address。
|
||||
"""
|
||||
global DEST_64
|
||||
global HELLO_SENT
|
||||
global DISC_PENDING
|
||||
|
||||
if not is_valid_addr64(src64):
|
||||
return False
|
||||
|
||||
src64 = bytes(src64)
|
||||
|
||||
if DEST_64 is None:
|
||||
DEST_64 = src64
|
||||
HELLO_SENT = False
|
||||
return True
|
||||
|
||||
if DEST_64 != src64:
|
||||
DEST_64 = src64
|
||||
HELLO_SENT = False
|
||||
DISC_PENDING = False
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
|
||||
def build_api_tx_frame(payload):
|
||||
"""
|
||||
建立 XBee API frame type 0x10，把 payload 送到 DEST_64。
|
||||
若 DEST_64 尚未學到，回傳 None。
|
||||
"""
|
||||
if DEST_64 is None:
|
||||
return None
|
||||
|
||||
frame_content = bytearray()
|
||||
frame_content.append(0x10)
|
||||
frame_content.append(0x00)
|
||||
frame_content.extend(DEST_64)
|
||||
frame_content.extend(b'\xFF\xFE')
|
||||
frame_content.append(0x00)
|
||||
frame_content.append(0x00)
|
||||
frame_content.extend(payload)
|
||||
|
||||
length = len(frame_content)
|
||||
|
||||
packet = bytearray()
|
||||
packet.append(0x7E)
|
||||
packet.append((length >> 8) & 0xFF)
|
||||
packet.append(length & 0xFF)
|
||||
packet.extend(frame_content)
|
||||
packet.append(get_checksum(frame_content))
|
||||
|
||||
return packet
|
||||
|
||||
|
||||
def send_to_xbee_chunked(payload):
|
||||
"""
|
||||
把 payload 依 XBEE_MAX_PAYLOAD 切成多個 XBee API TX frame 送出。
|
||||
"""
|
||||
if DEST_64 is None:
|
||||
return False
|
||||
|
||||
total_len = len(payload)
|
||||
sent_len = 0
|
||||
|
||||
while sent_len < total_len:
|
||||
end_len = min(sent_len + XBEE_MAX_PAYLOAD, total_len)
|
||||
chunk = payload[sent_len:end_len]
|
||||
sent_len = end_len
|
||||
|
||||
pkt = build_api_tx_frame(chunk)
|
||||
if pkt is None:
|
||||
return False
|
||||
|
||||
uart_xb.write(pkt)
|
||||
time.sleep_ms(1)
|
||||
|
||||
return True
|
||||
|
||||
|
||||
# =========================================================
|
||||
# HELO / DONE 回報
|
||||
# =========================================================
|
||||
|
||||
def send_hello_report(force=False):
|
||||
"""
|
||||
UAV 回報自己存在：
|
||||
HELO + MY_SYSID
|
||||
|
||||
本版 DISC 預設以 force=True 呼叫，因此每次收到 DISC 都會重新回 HELO。
|
||||
"""
|
||||
global _last_hello_ms
|
||||
global HELLO_SENT
|
||||
|
||||
if MY_SYSID is None:
|
||||
return False
|
||||
if DEST_64 is None:
|
||||
return False
|
||||
|
||||
if HELLO_SENT and not force:
|
||||
return False
|
||||
|
||||
now = time.ticks_ms()
|
||||
if not force:
|
||||
if time.ticks_diff(now, _last_hello_ms) < HELLO_MIN_INTERVAL_MS:
|
||||
return False
|
||||
|
||||
payload = HELLO_MAGIC + struct.pack('>B', MY_SYSID)
|
||||
pkt = build_api_tx_frame(payload)
|
||||
if pkt is None:
|
||||
return False
|
||||
|
||||
uart_xb.write(pkt)
|
||||
_last_hello_ms = now
|
||||
HELLO_SENT = True
|
||||
|
||||
time.sleep_ms(1)
|
||||
return True
|
||||
|
||||
|
||||
def try_send_pending_hello():
|
||||
"""
|
||||
若先收到 DISC、後學到 MY_SYSID，這裡會自動補送 HELO。
|
||||
"""
|
||||
global DISC_PENDING
|
||||
|
||||
if DISC_PENDING and MY_SYSID is not None and DEST_64 is not None:
|
||||
delay_ms = 20 + (((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180)
|
||||
time.sleep_ms(delay_ms)
|
||||
|
||||
if send_hello_report(force=True):
|
||||
DISC_PENDING = False
|
||||
|
||||
|
||||
def send_done_report(sent_len, remain_len):
|
||||
"""
|
||||
DONE payload:
|
||||
b'DONE' + sysid(1) + sent_len(2) + remain_len(2)
|
||||
"""
|
||||
if MY_SYSID is None:
|
||||
return False
|
||||
if DEST_64 is None:
|
||||
return False
|
||||
|
||||
if sent_len > 65535:
|
||||
sent_len = 65535
|
||||
if remain_len > 65535:
|
||||
remain_len = 65535
|
||||
|
||||
payload = DONE_MAGIC + struct.pack('>BHH', MY_SYSID, sent_len, remain_len)
|
||||
pkt = build_api_tx_frame(payload)
|
||||
if pkt is None:
|
||||
return False
|
||||
|
||||
uart_xb.write(pkt)
|
||||
time.sleep_ms(1)
|
||||
return True
|
||||
|
||||
|
||||
# =========================================================
|
||||
# MAVLink frame 解析與 MY_SYSID 自動學習
|
||||
# =========================================================
|
||||
|
||||
def find_first_mavlink_magic(buf):
|
||||
pos_fe = buf.find(b'\xFE')
|
||||
pos_fd = buf.find(b'\xFD')
|
||||
|
||||
if pos_fe == -1:
|
||||
return pos_fd
|
||||
if pos_fd == -1:
|
||||
return pos_fe
|
||||
return pos_fe if pos_fe < pos_fd else pos_fd
|
||||
|
||||
|
||||
def mavlink_frame_length(buf, start_idx):
|
||||
if start_idx >= len(buf):
|
||||
return None
|
||||
|
||||
magic = buf[start_idx]
|
||||
|
||||
if magic == 0xFE:
|
||||
if len(buf) - start_idx < 2:
|
||||
return None
|
||||
payload_len = buf[start_idx + 1]
|
||||
total_len = payload_len + 8
|
||||
if len(buf) - start_idx < total_len:
|
||||
return None
|
||||
return total_len
|
||||
|
||||
if magic == 0xFD:
|
||||
if len(buf) - start_idx < 3:
|
||||
return None
|
||||
payload_len = buf[start_idx + 1]
|
||||
incompat_flags = buf[start_idx + 2]
|
||||
signed = (incompat_flags & 0x01) != 0
|
||||
total_len = payload_len + 12 + (13 if signed else 0)
|
||||
if len(buf) - start_idx < total_len:
|
||||
return None
|
||||
return total_len
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def get_mavlink_sysid(buf, start_idx):
|
||||
if start_idx >= len(buf):
|
||||
return None
|
||||
|
||||
magic = buf[start_idx]
|
||||
|
||||
if magic == 0xFE:
|
||||
if len(buf) - start_idx >= 6:
|
||||
return buf[start_idx + 3]
|
||||
|
||||
if magic == 0xFD:
|
||||
if len(buf) - start_idx >= 10:
|
||||
return buf[start_idx + 5]
|
||||
|
||||
return None
|
||||
|
||||
|
||||
def learn_my_sysid_from_tx_buf():
|
||||
"""
|
||||
從 FC -> ESP32 的 tx_buf 中找完整 MAVLink frame，
|
||||
並自動學習飛控的 MAVLink SYSID。
|
||||
|
||||
若更改飛控 SYSID，建議重新上電 ESP32，讓 MY_SYSID 重新學習。
|
||||
"""
|
||||
global MY_SYSID
|
||||
global _sysid_candidate
|
||||
global _sysid_candidate_count
|
||||
global HELLO_SENT
|
||||
global tx_buf
|
||||
|
||||
if len(tx_buf) == 0:
|
||||
return
|
||||
|
||||
idx = 0
|
||||
checked = 0
|
||||
|
||||
while idx < len(tx_buf) and checked < 8:
|
||||
sub = tx_buf[idx:]
|
||||
rel_start = find_first_mavlink_magic(sub)
|
||||
if rel_start == -1:
|
||||
return
|
||||
|
||||
start = idx + rel_start
|
||||
frame_len = mavlink_frame_length(tx_buf, start)
|
||||
if frame_len is None:
|
||||
return
|
||||
|
||||
sysid = get_mavlink_sysid(tx_buf, start)
|
||||
|
||||
if sysid is not None and sysid > 0:
|
||||
if MY_SYSID is not None:
|
||||
return
|
||||
|
||||
if _sysid_candidate == sysid:
|
||||
_sysid_candidate_count += 1
|
||||
else:
|
||||
_sysid_candidate = sysid
|
||||
_sysid_candidate_count = 1
|
||||
|
||||
if _sysid_candidate_count >= SYSID_LEARN_CONFIRM_COUNT:
|
||||
MY_SYSID = _sysid_candidate
|
||||
HELLO_SENT = False
|
||||
return
|
||||
|
||||
idx = start + frame_len
|
||||
checked += 1
|
||||
|
||||
|
||||
# =========================================================
|
||||
# tx_buf MAVLink frame 取出與裁切
|
||||
# =========================================================
|
||||
|
||||
def pop_mavlink_frames_by_quota(quota_bytes):
|
||||
global tx_buf
|
||||
|
||||
if quota_bytes <= 0 or len(tx_buf) == 0:
|
||||
return b''
|
||||
|
||||
start = find_first_mavlink_magic(tx_buf)
|
||||
if start == -1:
|
||||
tx_buf = bytearray()
|
||||
return b''
|
||||
|
||||
if start > 0:
|
||||
tx_buf = tx_buf[start:]
|
||||
|
||||
out = bytearray()
|
||||
|
||||
while len(tx_buf) > 0:
|
||||
if tx_buf[0] not in (0xFE, 0xFD):
|
||||
start = find_first_mavlink_magic(tx_buf)
|
||||
if start == -1:
|
||||
tx_buf = bytearray()
|
||||
break
|
||||
tx_buf = tx_buf[start:]
|
||||
|
||||
frame_len = mavlink_frame_length(tx_buf, 0)
|
||||
if frame_len is None:
|
||||
break
|
||||
|
||||
if len(out) > 0 and (len(out) + frame_len) > quota_bytes:
|
||||
break
|
||||
|
||||
if len(out) == 0 and frame_len > quota_bytes:
|
||||
out.extend(tx_buf[:frame_len])
|
||||
tx_buf = tx_buf[frame_len:]
|
||||
break
|
||||
|
||||
out.extend(tx_buf[:frame_len])
|
||||
tx_buf = tx_buf[frame_len:]
|
||||
|
||||
if len(out) >= quota_bytes:
|
||||
break
|
||||
|
||||
return bytes(out)
|
||||
|
||||
|
||||
def trim_tx_buffer_if_needed():
|
||||
global tx_buf
|
||||
|
||||
if len(tx_buf) <= MAX_BUF_SIZE:
|
||||
return
|
||||
|
||||
target_size = MAX_BUF_SIZE // 2
|
||||
|
||||
while len(tx_buf) > target_size:
|
||||
start = find_first_mavlink_magic(tx_buf)
|
||||
if start == -1:
|
||||
tx_buf = bytearray()
|
||||
return
|
||||
|
||||
if start > 0:
|
||||
tx_buf = tx_buf[start:]
|
||||
continue
|
||||
|
||||
frame_len = mavlink_frame_length(tx_buf, 0)
|
||||
if frame_len is None:
|
||||
if len(tx_buf) > target_size:
|
||||
tx_buf = tx_buf[-target_size:]
|
||||
return
|
||||
|
||||
tx_buf = tx_buf[frame_len:]
|
||||
|
||||
|
||||
def flush_tx_buffer(grant_bytes):
|
||||
global tx_buf
|
||||
|
||||
if MY_SYSID is None: # TODO 這個重複判斷了 應該刪 不要浪費效率
|
||||
return
|
||||
|
||||
if DEST_64 is None:
|
||||
return
|
||||
|
||||
if grant_bytes <= 0:
|
||||
send_done_report(0, len(tx_buf))
|
||||
return
|
||||
|
||||
data_to_send = pop_mavlink_frames_by_quota(grant_bytes)
|
||||
sent_len = len(data_to_send)
|
||||
|
||||
if sent_len > 0:
|
||||
send_to_xbee_chunked(data_to_send)
|
||||
|
||||
send_done_report(sent_len, len(tx_buf))
|
||||
|
||||
|
||||
# =========================================================
|
||||
# GCS control payload 解析
|
||||
# =========================================================
|
||||
|
||||
def parse_poll_payload(real_data):
|
||||
if not real_data.startswith(POLL_MAGIC):
|
||||
return None, None
|
||||
|
||||
if len(real_data) == 5:
|
||||
return real_data[4], DEFAULT_GRANT_BYTES
|
||||
|
||||
if len(real_data) == 7:
|
||||
target_sysid = real_data[4]
|
||||
grant_bytes = (real_data[5] << 8) | real_data[6]
|
||||
return target_sysid, grant_bytes
|
||||
|
||||
return None, None
|
||||
|
||||
|
||||
def is_discovery_payload(real_data):
|
||||
return real_data.startswith(DISC_MAGIC)
|
||||
|
||||
|
||||
# =========================================================
|
||||
# XBee API RX parser
|
||||
# =========================================================
|
||||
|
||||
def process_xbee_buffer():
|
||||
global rx_buf
|
||||
global DISC_PENDING
|
||||
|
||||
while True:
|
||||
start_pos = rx_buf.find(b'\x7E')
|
||||
if start_pos == -1:
|
||||
rx_buf = bytearray()
|
||||
return
|
||||
|
||||
if start_pos > 0:
|
||||
rx_buf = rx_buf[start_pos:]
|
||||
|
||||
if len(rx_buf) < 3:
|
||||
return
|
||||
|
||||
pkt_len = (rx_buf[1] << 8) | rx_buf[2]
|
||||
total_len = pkt_len + 4
|
||||
|
||||
if pkt_len > 300:
|
||||
rx_buf = rx_buf[1:]
|
||||
continue
|
||||
|
||||
if len(rx_buf) < total_len:
|
||||
return
|
||||
|
||||
packet = rx_buf[3:3 + pkt_len]
|
||||
checksum_recv = rx_buf[3 + pkt_len]
|
||||
|
||||
if get_checksum(packet) == checksum_recv:
|
||||
if len(packet) > 0 and packet[0] == 0x90:
|
||||
# XBee Receive Packet 0x90:
|
||||
# packet = 90 | src64(8) | src16(2) | options(1) | RF data
|
||||
if len(packet) >= 12:
|
||||
src64 = bytes(packet[1:9])
|
||||
real_data = packet[12:]
|
||||
|
||||
# -------------------------------------------------
|
||||
# DISC：本版唯一 discovery 封包。
|
||||
# 功能等同原本 DSCF：收到後強制回 HELO。
|
||||
# -------------------------------------------------
|
||||
if is_discovery_payload(real_data):
|
||||
learn_dest64_from_src64(src64)
|
||||
learn_my_sysid_from_tx_buf() # TODO 加一個 if MY_SYSID is None
|
||||
|
||||
if MY_SYSID is not None and DEST_64 is not None:
|
||||
delay_ms = 20 + ((MY_SYSID * 37) + (time.ticks_ms() & 0xFF)) % 180 # TODO 這邊會有多少秒延遲? 為啥是37? 不能21或者17? 20 ~ 199 210 / 30000
|
||||
time.sleep_ms(delay_ms)
|
||||
send_hello_report(force=True)
|
||||
DISC_PENDING = False
|
||||
else:
|
||||
DISC_PENDING = True
|
||||
|
||||
# -------------------------------------------------
|
||||
# 判斷是不是 POLL
|
||||
# -------------------------------------------------
|
||||
else: # TODO 這邊要用 elif 去判斷 POLL
|
||||
target_sysid, grant_bytes = parse_poll_payload(real_data)
|
||||
|
||||
if target_sysid is not None:
|
||||
learn_dest64_from_src64(src64)
|
||||
learn_my_sysid_from_tx_buf()
|
||||
try_send_pending_hello() # TODO 這邊三行怪怪的 GCS 下 poll 然後會回應 HELO 這個很怪 然後為何要每次重做上面兩行 拖效率
|
||||
|
||||
if MY_SYSID is not None and target_sysid == MY_SYSID:
|
||||
flush_tx_buffer(grant_bytes)
|
||||
|
||||
else:
|
||||
# 一般 GCS -> FC MAVLink 下行資料
|
||||
if DEST_64 is None: # TODO 這個概念不對 隨便一個 poll 就把本來有做過 DISC 的權限搶走了 危險!!!
|
||||
learn_dest64_from_src64(src64)
|
||||
|
||||
if DEST_64 is not None and src64 == DEST_64: # TODO 從 GCS 到 FC 的 mavlink 封包 切開不要跟 POLL 放在同一個判斷
|
||||
uart_fc.write(real_data)
|
||||
|
||||
rx_buf = rx_buf[total_len:]
|
||||
|
||||
else:
|
||||
rx_buf = rx_buf[1:]
|
||||
|
||||
|
||||
|
||||
# =========================================================
|
||||
# 初始清理
|
||||
# =========================================================
|
||||
|
||||
try:
|
||||
gc.collect()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
# =========================================================
|
||||
# 主迴圈
|
||||
# =========================================================
|
||||
|
||||
while True:
|
||||
try:
|
||||
# -------------------------------------------------
|
||||
# FC -> ESP32 tx buffer
|
||||
# -------------------------------------------------
|
||||
if uart_fc.any():
|
||||
data = uart_fc.read(READ_FC_BYTES)
|
||||
if data:
|
||||
tx_buf.extend(data)
|
||||
|
||||
learn_my_sysid_from_tx_buf()
|
||||
try_send_pending_hello()
|
||||
trim_tx_buffer_if_needed()
|
||||
|
||||
# -------------------------------------------------
|
||||
# XBee -> ESP32
|
||||
# -------------------------------------------------
|
||||
if uart_xb.any():
|
||||
chunk = uart_xb.read()
|
||||
if chunk:
|
||||
rx_buf.extend(chunk)
|
||||
|
||||
process_xbee_buffer()
|
||||
|
||||
time.sleep_ms(1)
|
||||
|
||||
except MemoryError:
|
||||
tx_buf = bytearray()
|
||||
rx_buf = bytearray()
|
||||
|
||||
try:
|
||||
gc.collect()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
time.sleep_ms(10)
|
||||
|
||||
except Exception:
|
||||
time.sleep_ms(2)
|
||||
@@ -1,209 +0,0 @@
|
||||
# import socket
|
||||
|
||||
# def get_rtk2go_source_table():
|
||||
# host = "rtk2go.com"
|
||||
# port = 2101
|
||||
#
|
||||
# print(f"正在連線至 {host}:{port} 獲取掛載點列表...\n")
|
||||
# ...
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import socket
|
||||
import base64
|
||||
import time
|
||||
|
||||
|
||||
# ── NMEA GGA（告訴 Caster「我在哪」，觸發 RTCM 推流）────────────────
|
||||
|
||||
def nmea_checksum(body: str) -> str:
|
||||
"""body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'"""
|
||||
value = 0
|
||||
for ch in body:
|
||||
value ^= ord(ch)
|
||||
return f"{value:02X}"
|
||||
|
||||
|
||||
def decimal_to_nmea_dm(deg: float, *, is_latitude: bool) -> tuple[str, str]:
|
||||
"""十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。"""
|
||||
if is_latitude:
|
||||
hemi = "N" if deg >= 0 else "S"
|
||||
deg_width = 2
|
||||
else:
|
||||
hemi = "E" if deg >= 0 else "W"
|
||||
deg_width = 3
|
||||
deg = abs(deg)
|
||||
d = int(deg)
|
||||
m = (deg - d) * 60.0
|
||||
return f"{d:0{deg_width}d}{m:07.4f}", hemi
|
||||
|
||||
|
||||
def build_gga_sentence(lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes:
|
||||
"""
|
||||
組一筆 $GPGGA 句子（含 checksum），回傳 bytes 可直接 sock.sendall。
|
||||
|
||||
lat_deg / lon_deg：十進位經緯度（北、東為正）。
|
||||
測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。
|
||||
"""
|
||||
utc = time.gmtime()
|
||||
t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00"
|
||||
|
||||
lat_dm, ns = decimal_to_nmea_dm(lat_deg, is_latitude=True)
|
||||
lon_dm, ew = decimal_to_nmea_dm(lon_deg, is_latitude=False)
|
||||
|
||||
# quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意
|
||||
body = (
|
||||
f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew},"
|
||||
f"1,08,1.0,{alt_m:.1f},M,0.0,M,,"
|
||||
)
|
||||
sentence = f"${body}*{nmea_checksum(body)}\r\n"
|
||||
return sentence.encode("ascii")
|
||||
|
||||
|
||||
def send_gga(sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str:
|
||||
"""送出 GGA，回傳可讀句子供列印。"""
|
||||
payload = build_gga_sentence(lat_deg, lon_deg, alt_m)
|
||||
sock.sendall(payload)
|
||||
return payload.decode("ascii").strip()
|
||||
|
||||
|
||||
# ── NTRIP 連線 + RTCM 接收 ─────────────────────────────────────────
|
||||
|
||||
def get_rtcm_payload(
|
||||
host,
|
||||
port,
|
||||
mountpoint,
|
||||
user="",
|
||||
password="",
|
||||
target_ids=None,
|
||||
*,
|
||||
gga_lat=None,
|
||||
gga_lon=None,
|
||||
gga_alt_m=100.0,
|
||||
gga_interval_sec=5.0,
|
||||
filter_ids=True,
|
||||
):
|
||||
"""
|
||||
連線 NTRIP caster 並解析 RTCM3。
|
||||
|
||||
gga_lat / gga_lon：若兩者皆有設定，握手成功後會週期送出 GGA（許多 Caster 如 SNIP 至少需一次）。
|
||||
gga_interval_sec：GGA 重送間隔（秒）。
|
||||
filter_ids：True 時只印 target_ids 內的 message；False 印全部（方便學習觀察）。
|
||||
"""
|
||||
if target_ids is None:
|
||||
target_ids = []
|
||||
need_gga = gga_lat is not None and gga_lon is not None
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
sock.settimeout(10)
|
||||
|
||||
try:
|
||||
sock.connect((host, port))
|
||||
|
||||
auth = base64.b64encode(f"{user}:{password}".encode()).decode()
|
||||
request = f"GET /{mountpoint} HTTP/1.0\r\n"
|
||||
request += "User-Agent: NTRIP PythonClient\r\n"
|
||||
request += f"Authorization: Basic {auth}\r\n"
|
||||
request += "Connection: close\r\n\r\n"
|
||||
sock.sendall(request.encode())
|
||||
|
||||
response = sock.recv(4096)
|
||||
if b"ICY 200 OK" not in response:
|
||||
print(f"連線失敗: {response.decode(errors='ignore')}")
|
||||
return
|
||||
|
||||
print(f"已成功連接至 {mountpoint}")
|
||||
if need_gga:
|
||||
print(
|
||||
f"GGA 模式：每 {gga_interval_sec}s 回報位置 "
|
||||
f"({gga_lat:.6f}, {gga_lon:.6f})，觸發 Caster 推流"
|
||||
)
|
||||
else:
|
||||
print("未設定 GGA（部分 Caster 如 RTK2GO 仍可推流；SNIP/區域站常需 GGA）")
|
||||
print(f"RTCM 過濾目標: {target_ids if filter_ids else '(關閉，顯示全部)'}\n")
|
||||
|
||||
last_gga_time = 0.0
|
||||
buffer = b""
|
||||
|
||||
while True:
|
||||
now = time.time()
|
||||
if need_gga and (now - last_gga_time >= gga_interval_sec):
|
||||
line = send_gga(sock, gga_lat, gga_lon, gga_alt_m)
|
||||
print(f"[GGA] {line}")
|
||||
last_gga_time = now
|
||||
|
||||
try:
|
||||
chunk = sock.recv(4096)
|
||||
except socket.timeout:
|
||||
continue
|
||||
|
||||
if not chunk:
|
||||
print("Caster 關閉連線")
|
||||
break
|
||||
|
||||
buffer += chunk
|
||||
|
||||
while len(buffer) >= 3:
|
||||
if buffer[0] != 0xD3:
|
||||
buffer = buffer[1:]
|
||||
continue
|
||||
|
||||
length = ((buffer[1] & 0x03) << 8) | buffer[2]
|
||||
total_len = length + 6
|
||||
|
||||
if len(buffer) < total_len:
|
||||
break
|
||||
|
||||
packet = buffer[:total_len]
|
||||
buffer = buffer[total_len:]
|
||||
msg_id = (packet[3] << 4) | (packet[4] >> 4)
|
||||
|
||||
if not filter_ids or str(msg_id) in target_ids:
|
||||
print(f"Found ID: {msg_id} | Length: {total_len} bytes")
|
||||
|
||||
except KeyboardInterrupt:
|
||||
print("\n使用者中斷")
|
||||
except Exception as e:
|
||||
print(f"發生錯誤: {e}")
|
||||
finally:
|
||||
sock.close()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
TARGET_LIST = ["1007", "1230", "1074"]
|
||||
|
||||
# ── Case 1：RTK2GO（通常可不送 GGA）────────────────────────────
|
||||
# USER = "chiyu1468@hotmail.com"
|
||||
# HOST = "rtk2go.com"
|
||||
# PORT = 2101
|
||||
# MOUNT = "No1bio_02"
|
||||
# get_rtcm_payload(
|
||||
# HOST, PORT, MOUNT,
|
||||
# user=USER,
|
||||
# target_ids=TARGET_LIST,
|
||||
# filter_ids=False,
|
||||
# )
|
||||
|
||||
# ── Case 2：區域 Caster（需 GGA，座標請改成服務區內近似點）──────
|
||||
USER = "uavlab6061"
|
||||
PW = "iamsupersmart"
|
||||
HOST = "210.241.63.193"
|
||||
PORT = 81
|
||||
MOUNT = "2020_GNSS"
|
||||
# 十進位度；請依 SNIP / 基準站服務範圍調
|
||||
GGA_LAT = 24.155792000
|
||||
GGA_LON = 120.630679000
|
||||
|
||||
get_rtcm_payload(
|
||||
HOST,
|
||||
PORT,
|
||||
MOUNT,
|
||||
user=USER,
|
||||
password=PW,
|
||||
target_ids=TARGET_LIST,
|
||||
gga_lat=GGA_LAT,
|
||||
gga_lon=GGA_LON,
|
||||
gga_alt_m=100.0,
|
||||
gga_interval_sec=30.0,
|
||||
filter_ids=False,
|
||||
)
|
||||
@@ -1,33 +0,0 @@
|
||||
import timeit
|
||||
|
||||
# 建立 1000 個元素的字典，避免單一 Key 的特殊雜湊值干擾
|
||||
d_int = {i: True for i in range(1000)}
|
||||
d_bytes = {f"k_{i:06d}".encode(): True for i in range(1000)}
|
||||
|
||||
# 正確命中中間的值
|
||||
target_int = 500
|
||||
target_bytes_same = f"k_{500:06d}".encode()
|
||||
|
||||
# 將變數傳入 timeit 的全域環境
|
||||
setup_env = lambda: None
|
||||
globals_dict = {
|
||||
'd_int': d_int,
|
||||
'd_bytes': d_bytes,
|
||||
'target_int': target_int,
|
||||
'target_bytes_same': target_bytes_same
|
||||
}
|
||||
|
||||
# 1. int 查詢
|
||||
t_int = timeit.timeit("d_int.get(target_int)", globals=globals_dict, number=50000000)
|
||||
|
||||
# 2. bytes 查詢 (同物件：直接傳入現成變數)
|
||||
t_bytes_same = timeit.timeit("d_bytes.get(target_bytes_same)", globals=globals_dict, number=50000000)
|
||||
|
||||
# 3. bytes 查詢 (真・新物件：每次都在查詢時動態切片生成新物件，強迫重新計算 Hash)
|
||||
globals_dict['dynamic_source'] = f"k_{500:06d}".encode() + b"dummy"
|
||||
t_bytes_new = timeit.timeit("d_bytes.get(dynamic_source[:8])", globals=globals_dict, number=50000000)
|
||||
|
||||
print(f"修正後測試結果 (50,000,000 次)：")
|
||||
print(f"int 查詢 : {t_int:.4f} 秒 (1.0x)")
|
||||
print(f"bytes 查詢 (同物件): {t_bytes_same:.4f} 秒 (約 {t_bytes_same/t_int:.2f}x)")
|
||||
print(f"bytes 查詢 (新物件): {t_bytes_new:.4f} 秒 (約 {t_bytes_new/t_int:.2f}x)")
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user