temp commit for share code

src/unitdev02/unitdev02/test01.py

@@ -42,6 +42,11 @@ def mavlink_analyzer_simple(count = 500):
             print('No message yet, 1 second for data to fill')
             sleep(1)
 
+    print("markA ------")
+    print(msg.get_type())
+    print(msg.ordered_fieldnames)
+    print("markA ------ END")
+
     print('Messages Type received:')
     print(msg_count)
 
@@ -108,7 +113,8 @@ def fdm_switch_example_one():
         elapsed_time = current_time - start_time
 
         if elapsed_time >= 1.0:
-            print(f"Packets received in the last second: {packet_count}")
+            # print(f"Packets received in the last second: {packet_count}")
+            print(f'JSON Pack : {data_g2s}')
             packet_count = 0
             start_time = current_time
             fdm_parser_example(data_s2g)
@@ -132,24 +138,24 @@ def fdm_switch_example_two():
     server_address = ('127.0.0.1', 19002)
 
     data_queue = queue.Queue()
-    servo_out = [0] * 16
+    servo_out = [0] * 16 # [0 0 0 0 0 0 0 0 0.....]
     data_queue.put(servo_out)
 
-    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1):
+    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=1, frame_count_fdm=1):
         interval = 1.0 / frame_rate_fdm
         while True:
             start_time = time.time()
             try:
-                data = q.get(timeout=0.1)
+                data = q.get(timeout=0.1) # [0 0 0 0 0 0 0 0 0.....]
                 if data is None:
                     break
                 last_data = data
             except queue.Empty:
                 data = last_data
-            data = struct.pack('HHI16H', 0x481a, frame_rate_fdm, frame_count_fdm, *servo_out)
-            sock.sendto(data, server_address)
-            frame_count_fdm += 1
-            fdm_parser_example(data) # debug
+            data_fdm = struct.pack('HHI16H', 0x481a, int(frame_rate_fdm * 0.1), frame_count_fdm, *servo_out) # [ FMD ]
+            sock.sendto(data_fdm, server_address)
+            frame_count_fdm += 3
+            # fdm_parser_example(data_fdm) # debug
 
             elapsed_time = time.time() - start_time
             sleep_time = interval - elapsed_time
@@ -159,6 +165,7 @@ def fdm_switch_example_two():
     Running = True
     thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
     thread.start()
+    print('Start sending data to Gazebo')
 
     while Running:
         msgb1 = None
@@ -184,7 +191,39 @@ def fdm_switch_example_two():
         # To stop the thread, you can put None into the queue
         # data_queue.put(None)
 
+from pymavlink.dialects.v20 import common  # 使用 MAVLink 2.0
+
+def mavlink_btye_generator_test():
+
+    # 創建一個 MAVLink 對象
+    mav = common.MAVLink(None)  # 不透過 connection，直接使用 None
+
+    # 創建一個 HEARTBEAT 訊息
+    msg = mav.heartbeat_encode(
+        type=mavutil.mavlink.MAV_TYPE_QUADROTOR,
+        autopilot=mavutil.mavlink.MAV_AUTOPILOT_GENERIC,
+        base_mode=0,
+        custom_mode=0,
+        system_status=mavutil.mavlink.MAV_STATE_ACTIVE
+    )
+
+    # 取得 MAVLink 訊息的 bytes
+    mavlink_bytes = msg.pack(mav)
+
+    # 回傳 bytes
+    print(mavlink_bytes)
+    print(type(mavlink_bytes))
+
+    mav2 = common.MAVLink(None)
+    # my_msg = mav2.decode(mavlink_bytes)
+    print("========")
+    print(dir(mav2))
+    # print(my_msg)
+
+
 
 print('Start')
 # fdm_switch_example_two()
-# fdm_parser_example()
+# fdm_parser_example()
+# mavlink_analyzer_simple(8)
+mavlink_btye_generator_test()