diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index b76aa89..185bea2 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -30,6 +30,7 @@ from theLogger import setup_logger # ====================== 分割線 ===================== + logger = setup_logger("mavlinkObject.py") fixed_stream_analyzer_queue = queue.Queue() @@ -40,107 +41,7 @@ converte_format_queues = [] # 資料格式 { sysid : mavlink_device object } mavlink_systems = {} -class mavlink_object(): - def __init__(self, mavlink_socket, socket_id = None): - self.socket_id = socket_id - self.mavlink_socket = mavlink_socket - self.msg_count = {} - self.multiplexingList = [] - - # 關聯到全域變數 - global mavlink_systems - self.mavlink_systems = mavlink_systems - - # 這三個 list 用來分配不同的訊息到不同的 queue - self.multiplexingToAnalysis = [ - 0, # HEARTBEAT - 24, # GPS_RAW_INT - 30, # ATTITUDE - 33, # GLOBAL_POSITION_INT - 74, # VFR_HUD - 147, # BATTERY_STATUS - ] - self.multiplexingToReturn = [ - 0, # HEARTBEAT - # 30, # ATTITUDE - ] - self.multiplexingToConvert = [ - ] - def __del__(self): - self.mavlink_socket.close() - self.stop() - - def run(self): - if not self.socket_id: - logger.error('Please set socket id before running') - return - self.thread = threading.Thread(target=self._run_thread) - self.running = self.updateMultiplexingList() - self.thread.start() - - def stop(self): - self.running = False - - def _run_thread(self): - logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) - start_time = time.time() - while self.running: - timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 - try: - mavlinkMsg = self.mavlink_socket.recv_msg() - except Exception as e: - logger.critical(f"Receiving data not mavlink format. Object Delete.") - print(f"[mavlinkObject.py] Error receiving mavlink data: {e}") - print(mavlinkMsg) - self.running = False - break - - if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' - # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 - sysid = mavlinkMsg.get_srcSystem() - compid = mavlinkMsg.get_srcComponent() - if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息 - # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 - mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) - - # print(mavlinkMsg) # debug - # break # Debug - - # 將訊息依照 multiplexing list 分發到不同的 queue - for i in range(len(self.multiplexingList)): - if mavlinkMsg.get_msgId() in self.multiplexingList[i]: - if i == 0: - fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg)) - elif i == 1: - return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) - else: - convert_queue = converte_format_queues[i-2] - convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) - - # thread 結束 - logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) - - def updateMultiplexingList(self): - ''' - 更新 multiplexing list 並做簡單的檢查 - ''' - - # 更新 multiplexing list - self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert - - # 檢查 multiplexing list 格式是否有錯誤 - check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) - if not check: - logger.error('Multiplexing List Format Error, Please check the list') - return False # 若有錯誤則回傳 False - - check = len(self.multiplexingToConvert) != len(converte_format_queues) - if check: - logger.error('Multiplexing Queue not fit List , Please check the list') - return False - return True - -# ===================== 分割線 ===================== +# ====================== 分割線 ===================== class mavlink_analyzer(Node, mavlink_publisher): ''' @@ -170,6 +71,7 @@ class mavlink_analyzer(Node, mavlink_publisher): if cls._instance is None: cls._instance = super(mavlink_analyzer, cls).__new__(cls) return cls._instance + def __init__(self): if not hasattr(self, "initialized"): # 防止重複初始化 self.initialized = True @@ -184,6 +86,7 @@ class mavlink_analyzer(Node, mavlink_publisher): self.thread.start() else: logger.error('mavlink_analyzer instance already exists. Do not create another one.') + def stop(self): self.running = False @@ -253,8 +156,127 @@ class mavlink_analyzer(Node, mavlink_publisher): # 再註銷 node pass +# ====================== 分割線 ===================== + +class mavlink_object(): + ''' + 每個 mavlink bus 都會有一個 mavlink_object + 其中主要是 thread 做統計封包與分流 + 分流表的控制在三個 list 分別為 + multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的 + multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的 + multiplexingToConvert : 這個 list 是用來分流到轉格式的 + 透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為 + ''' + def __init__(self, mavlink_socket, socket_id = None): + self.socket_id = socket_id + self.mavlink_socket = mavlink_socket + self.msg_count = {} + self.multiplexingList = [] + + # 關聯到全域變數 + global mavlink_systems + self.mavlink_systems = mavlink_systems + + # 這三個 list 用來分配不同的訊息到不同的 queue + self.multiplexingToAnalysis = [ + 0, # HEARTBEAT + 24, # GPS_RAW_INT + 30, # ATTITUDE + 33, # GLOBAL_POSITION_INT + 74, # VFR_HUD + 147, # BATTERY_STATUS + ] + self.multiplexingToReturn = [ + 0, # HEARTBEAT + # 30, # ATTITUDE + ] + self.multiplexingToConvert = [ + ] + + def __del__(self): + self.mavlink_socket.close() + self.stop() + + def run(self): + if not self.socket_id: + logger.error('Please set socket id before running') + return + self.thread = threading.Thread(target=self._run_thread) + self.running = self.updateMultiplexingList() + self.thread.start() + + def stop(self): + self.running = False + + def _run_thread(self): + logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id)) + start_time = time.time() + while self.running: + timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差 + try: + mavlinkMsg = self.mavlink_socket.recv_msg() + except Exception as e: + logger.critical(f"Receiving data not mavlink format. Object Delete.") + print(f"[mavlinkObject.py] Error receiving mavlink data: {e}") + print(mavlinkMsg) + self.running = False + break + + if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' + # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間 + sysid = mavlinkMsg.get_srcSystem() + compid = mavlinkMsg.get_srcComponent() + if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息 + # mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型 + mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp) -def main(args=None): + # print(mavlinkMsg) # debug + # break # Debug + + # 將訊息依照 multiplexing list 分發到不同的 queue + for i in range(len(self.multiplexingList)): + if mavlinkMsg.get_msgId() in self.multiplexingList[i]: + if i == 0: + fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg)) + elif i == 1: + return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg)) + else: + convert_queue = converte_format_queues[i-2] + convert_queue.put((self.socket_id,timestamp,mavlinkMsg)) + + # thread 結束 + logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id)) + + def updateMultiplexingList(self): + ''' + 更新 multiplexing list 並做簡單的檢查 + ''' + + # 更新 multiplexing list + self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + + # 檢查 multiplexing list 格式是否有錯誤 + check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) + if not check: + logger.error('Multiplexing List Format Error, Please check the list') + return False # 若有錯誤則回傳 False + + check = len(self.multiplexingToConvert) != len(converte_format_queues) + if check: + logger.error('Multiplexing Queue not fit List , Please check the list') + return False + return True + +# ====================== 分割線 ===================== + +# 整合到 ros2 之後的程式進入點 +def main_node(): + pass + +# ====================== 分割線 ===================== +# 測試時的程式進入點 +def main_develop(args=None): test_item = 2 print('test_item : ', test_item) @@ -411,4 +433,4 @@ def main(args=None): if __name__ == '__main__': import rclpy # 這邊是為了測試而加的 - main() + main_develop()