diff --git a/README.md b/README.md index b92299c..d03f8a7 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,10 @@ === Package 簡述 1. unitdev 為各自協作者做開發時的測試區 + 01 -> 晉凱(ken) + 02 -> 其宇(chiyu) + 03 -> 文鈞 + 04 -> 倫渝 2. fc_network_adapter 建立、維護與飛控韌體的連接 構築 mavlink 封包 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 093e8ac..f84ce60 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -1,5 +1,6 @@ import threading from pymavlink import mavutil +import queue import rclpy from rclpy.node import Node @@ -7,6 +8,7 @@ from std_msgs.msg import String import time + ''' # 不同的匯流排只有單一種通訊協定 @@ -19,24 +21,45 @@ import time # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 # 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function + ''' +fixed_stream_analyzer_queue = queue.Queue() +return_packet_processor_queue = queue.Queue() +converte_format_queues = [] + class MavlinkObject(): - def __init__(self, mavlink_socket): + def __init__(self, mavlink_socket, socket_id = None): + self.socket_id = socket_id self.mavlink_socket = mavlink_socket self.msg_count = {} - # self.multiplexingDict = {} - self.multiplexingToAnalysis = [] - self.multiplexingToReturn = [] - self.multiplexingToConvert = [] + self.multiplexingList = [] + + self.multiplexingToAnalysis = [ + # 0, # HEARTBEAT + 30, # ATTITUDE + 33, # GLOBAL_POSITION_INT + # 147, # BATTERY_STATUS + ] + self.multiplexingToReturn = [ + 0, # HEARTBEAT + # 30, # ATTITUDE + ] + self.multiplexingToConvert = [ + [74, ] # VFR_HUD + ] def __del__(self): self.mavlink_socket.close() self.stop() def run(self): + if not self.socket_id: + print('[mavlinkObject.py] Please set socket id before running') + return self.thread = threading.Thread(target=self._run_thread) - self.running = True + self.running = self.updateMultiplexingList() self.thread.start() + def stop(self): self.running = False @@ -45,24 +68,38 @@ class MavlinkObject(): while self.running: try: mavlinkMsg = self.mavlink_socket.recv_msg() - if mavlinkMsg: - self.count_mavlink_type(mavlinkMsg) - # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug - # break # Debug - except Exception as e: - print(f"Error receiving mavlink data: {e}") + print(f"[mavlinkObject.py] Error receiving mavlink data: {e}") print(mavlinkMsg) self.running = False break + if mavlinkMsg: + self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息 + # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # 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,mavlinkMsg)) + elif i == 1: + return_packet_processor_queue.put((self.socket_id,mavlinkMsg)) + else: + convert_queue = converte_format_queues[i-2] + convert_queue.put((self.socket_id,mavlinkMsg)) + # 每秒中 統計一次 收到的訊息總量 elapsed_time = time.time() - start_time if elapsed_time > 1: - print('Messages Type received (in about 1 sec) :') + print('[mavlinkObject.py] Messages Type received (in about 1 sec) :') print(self.msg_count) start_time = time.time() self.msg_count = {} + + # thread 結束 + print('[mavlinkObject.py] End of _run_thread') def count_mavlink_type(self, mavlinkMsg): msg_type = mavlinkMsg.get_type() @@ -76,6 +113,45 @@ class MavlinkObject(): msg.data = str(mavlinkMsg) self.publisher_.publish(msg) + def updateMultiplexingList(self): + # 更新 multiplexing list + self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert + + check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList) + + # 若有錯誤則回傳 False + if not check: + print('[mavlinkObject.py] Multiplexing List Error, Please check the list') + return False + return True + +class mavlink_analyzer(Node): + ''' + 這個 class 是用來接收 mavlink 訊息 並進行分析 + 記錄有 mavlink bus 上有那些 system id 和 component id + 為了每個 system id 都有一個對應的 device object + 並且看是否有重複 system id + + 也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒? + ''' + def __init__(self): + super().__init__('mavlink_analyzer') + + # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10) + # self.mavlink_object = None + # self.create_mavlink_object() + + def create_mavlink_object(self): + # connection_string="udp: + pass + +class mavlink_device(): + def __init__(self): + self.socket_id = None # 記錄來自於哪個 socket + self.sysid = None + self.component_count = 1 + self.compid_list = [] + def main(args=None): @@ -83,10 +159,34 @@ def main(args=None): connection_string="udp:127.0.0.1:14550" mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object = MavlinkObject(mavlink_socket) + # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來 + mavlink_object = MavlinkObject(mavlink_socket, 1) + mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE + mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT + mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD + + converte_format_queues.append(queue.Queue()) + mavlink_object.run() + start_time = time.time() + while time.time() - start_time < 2: + + while not fixed_stream_analyzer_queue.empty(): + print('fixed_stream_analyzer_queue:') + t = fixed_stream_analyzer_queue.get() + print('from {} : {}'.format(t[0],t[1])) + while not return_packet_processor_queue.empty(): + print('return_packet_processor_queue:') + t = return_packet_processor_queue.get() + print('from {} : {}'.format(t[0],t[1])) + + for q in converte_format_queues: + while not q.empty(): + print('converte_format_queues:') + t = q.get() + print('from {} : {}'.format(t[0],t[1])) + - time.sleep(5) mavlink_object.running = False mavlink_object.thread.join() print('End of Program') diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index bf634e2..c1b5276 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/src/unitdev02/unitdev02/test01.py @@ -62,7 +62,6 @@ def fdm_parser_example(data=None): data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000') # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000 - parse_format = 'HHI16H' if len(data) != struct.calcsize(parse_format): @@ -226,4 +225,6 @@ print('Start') # fdm_switch_example_two() # fdm_parser_example() # mavlink_analyzer_simple(8) -mavlink_btye_generator_test() +# mavlink_btye_generator_test() + +