From 0ce78b964a8744b2952c86ffe0d7a87bbefc2358 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 1 Dec 2025 21:24:56 +0800 Subject: [PATCH] before merge change nothing --- .../fc_network_adapter/mavlinkDevice.py | 98 ------------------- 1 file changed, 98 deletions(-) delete mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py deleted file mode 100644 index 2d7ea06..0000000 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ /dev/null @@ -1,98 +0,0 @@ - -import os - -# 自定義的 import -from .utils import setup_logger - -# ====================== 分割線 ===================== - -logger = setup_logger(os.path.basename(__file__)) - -# 用來記錄每個 system 的資訊 -# 資料格式 { sysid : mavlink_device object } -mavlink_systems = {} - -# ====================== 分割線 ===================== - -# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component -class mavlink_device(): - def __init__(self): - self.socket_id = None # 記錄來自於哪個 socket - self.sysid = None - self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object - - def __str__(self): - p_str = '=====================\n' - p_str += f'object id : {id(self)}\n' # debug - p_str += f'socket_id : {self.socket_id}\n' - p_str += f'sysid : {self.sysid}\n' - p_str += 'has components : \n' - for compid in self.components: - p_str += f'compid : {compid}\n' - p_str += f'mav_type : {self.components[compid].mav_type}\n' - # p_str += '=====================\n' - return p_str - - ''' - 寫了半天 這個功能應該是 pymalink 本來就有的 - 去找 pymavlink_util.py 的 mavfile(object) - 算了 先擺著吧 之後再看看怎麼整合 - ''' - def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): - # 這段檢查遺失封包 - try: - last_seq = self.components[compid].msg_seq - except KeyError: - # 這個 component id 還不存在 - # logger.error('System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid)) # 因為初始化的之前 會有大量非 heartbeat 的訊息進來 這是正常現象 TODO 之後要幫這個類別加上初始化狀態 再進行這個判斷 - return - - if last_seq != None: - expected_seq = (last_seq + 1) % 256 - diff = current_seq - expected_seq - # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug - if diff < 0: - diff += 256 - self.components[compid].lost_packet_count += diff - - # 這段更新封包的基本資訊 - self.components[compid].msg_seq = current_seq - self.components[compid].last_msg_time = current_time - if current_type in self.components[compid].msg_count: - self.components[compid].msg_count[current_type] += 1 - else: - self.components[compid].msg_count[current_type] = 1 - - def resetComponentPacketCount(self, compid): - self.components[compid].msg_count = {} - self.components[compid].msg_seq = None - self.components[compid].lost_packet_count = 0 - - class mavlink_component(): - # 程式用不到的參數 但是做個記錄 - # paraEmitList = ['base_mode', 'flightMode_mode', - # 'battery_voltage', # from BATTERY_STATUS (147) - # 'gps_fix_type', # from GPS_RAW_INT (24) - # 'roll', 'pitch', 'yaw', # from ATTITUDE (30) - # 'position_latitude', 'position_longitude', 'position_altitude', # from GLOBAL_POSITION_INT (33) - # 'heading', # for VFR_HUD (74) - # ] - - def __init__(self): - self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) - self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) - self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) - - # 紀錄從這個 component 收到最後的訊息序號和時間 - self.msg_count = {} - self.msg_seq = None - self.lost_packet_count = 0 - self.last_msg_time = 0 - - # 存放該 emit component 參數的區域 - # 內容格式為 {param_name(字串) : param_value} - # param_name 請見上面 paraEmitList - self.emitParams = {} - # 用來存放每個 topic 的 publisher - # 內容格式 為 {topic_name(字串) : [publisher(物件), method(函式)]} (? - self.publishers = {} \ No newline at end of file