src/unitdev01/unitdev01/mavlink.py

@@ -94,23 +94,23 @@ class MAVLinkWorker(QThread):
 
     def arm(self, namespace, arm):
         sysid = int(namespace[-1])
-        # self.connection.mav.command_long_send(
-        #     sysid,
-        #     1,  # Component ID
-        #     mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-        #     0,  # Confirmation
-        #     1 if arm else 0,
-        #     0, 0, 0, 0, 0, 0
-        # )
         self.connection.mav.command_long_send(
             sysid,
             1,  # Component ID
-            mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-            0,
+            0,  # Confirmation
-            33,
+            1 if arm else 0,
-            1000000, 
+            0, 0, 0, 0, 0, 0
-            0, 0, 0, 0, 0
         )
+        # self.connection.mav.command_long_send(
+        #     sysid,
+        #     1,  # Component ID
+        #     mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
+        #     0,
+        #     33,
+        #     -1, 
+        #     0, 0, 0, 0, 0
+        # )
 
     def takeoff(self, namespace, altitude):
         sysid = int(namespace[-1])

src/unitdev02/setup.py

@@ -21,8 +21,7 @@ setup(
     entry_points={
         'console_scripts': [
             'showmavsimple = unitdev02.test01:mavlink_analyzer_simple',
-            'fdm_switch_one = unitdev02.test01:fdm_switch_example_one',
+            'fdm_switch_one = unitdev02.test01:fdm_swicth_example_one',
-            'fdm_switch_two = unitdev02.test01:fdm_switch_example_two',
         ],
     },
 )

src/unitdev02/unitdev02/test01.py

@@ -6,10 +6,6 @@ import time
 import socket
 import struct
 
-# used at fdm_switch_example_two
-import threading
-import queue
-
 def mavlink_analyzer_simple(count = 500):
     # rclpy.init()
     # node = rclpy.create_node('mavlink_analyzer_simple')
@@ -36,7 +32,6 @@ def mavlink_analyzer_simple(count = 500):
 
             if msg.get_type() == 'SERVO_OUTPUT_RAW':
                 print(msg)
-                pass
 
         else:
             print('No message yet, 1 second for data to fill')
@@ -118,73 +113,34 @@ def fdm_switch_example_two():
     '''
     這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器 
     有轉換數據格式
-
-    time_usec
-    servo1_raw 
-    servo16_raw
     '''
     # read info from SITL via MAVLink
-    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
+    connection_string="udp:127.0.0.1:14550"
     connection = mavutil.mavlink_connection(connection_string)
 
     # set a target link pass JSON to Gazebo, as a client
     sock_g2s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     server_address = ('127.0.0.1', 19002)
 
-    data_queue = queue.Queue()
-    servo_out = [0] * 16
-    data_queue.put(servo_out)
-
-    def send_data_from_queue(sock, server_address, q, frame_rate_fdm=10, frame_count_fdm=1):
-        interval = 1.0 / frame_rate_fdm
-        while True:
-            start_time = time.time()
-            try:
-                data = q.get(timeout=0.1)
-                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
-
-            elapsed_time = time.time() - start_time
-            sleep_time = interval - elapsed_time
-            if sleep_time > 0:
-                sleep(sleep_time)
-
-    Running = True
-    thread = threading.Thread(target=send_data_from_queue, args=(sock_g2s, server_address, data_queue))
-    thread.start()
-
-    while Running:
-        msgb1 = None
-        msg = connection.recv_msg()
-        while msg :
-            if msg.get_type() == 'SERVO_OUTPUT_RAW':
-                msgb1 = msg
-                # break # 這裡不需要break，因為我要最後一個 servo_output_raw 的值
-            msg = connection.recv_msg()
-
-        if msgb1:
-            for i in range(16):
-                servo_out[i] = getattr(msgb1, f'servo{i+1}_raw')
-                data_queue.put(servo_out)
-            msgb1 = None
-            # Running = False # debug
-        else:
-            # print('No message yet, 10 ms for data to fill')
-            sleep(0.01)
-
-        # Example of putting data into the queue
-        # data_queue.put(data)
-        # To stop the thread, you can put None into the queue
-        # data_queue.put(None)
-
 
 print('Start')
-# fdm_switch_example_two()
+mavlink_analyzer_simple()
-# fdm_parser_example()
+
+# connection_string="udp:127.0.0.1:14550"
+# connection = mavutil.mavlink_connection(connection_string)
+
+# connection.mav.command_long_send(
+#     1,
+#     1,
+#     mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,
+#     0, 33, 1000000, 0, 0, 0, 0, 0
+# )
+
+# connection.mav.command_long_send(
+#         1,
+#         1,  # Component ID
+#         mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+#         0,  # Confirmation
+#         1,  # 0: disarm, 1: arm
+#         0, 0, 0, 0, 0, 0
+#     )