mavlink to gazebo devtest 02

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,
-        #     0,
-        #     33,
-        #     -1, 
-        #     0, 0, 0, 0, 0
+        #     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,
+            0,
+            33,
+            1000000, 
+            0, 0, 0, 0, 0
+        )
 
     def takeoff(self, namespace, altitude):
         sysid = int(namespace[-1])

src/unitdev02/setup.py

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

src/unitdev02/unitdev02/test01.py

@@ -6,6 +6,10 @@ 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')
@@ -32,6 +36,7 @@ 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')
@@ -113,34 +118,73 @@ 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"
+    connection_string="udp:127.0.0.1:14550" # uart mavlink  # MATLAB
     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')
-mavlink_analyzer_simple()
-
-# 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
-#     )
+# fdm_switch_example_two()
+# fdm_parser_example()