From cd9d055409cc03646b0537f777476e7fcaf702d1 Mon Sep 17 00:00:00 2001 From: picard Date: Fri, 7 Mar 2025 16:41:05 +0800 Subject: [PATCH] temp commit for share code --- README.md | 6 ++ .../fc_network_adapter/__init__.py | 0 .../fc_network_adapter/mavlinkObject.py | 97 +++++++++++++++++++ .../fc_network_adapter/socketManager.py | 14 +++ src/fc_network_adapter/package.xml | 18 ++++ .../resource/fc_network_adapter | 0 src/fc_network_adapter/setup.cfg | 4 + src/fc_network_adapter/setup.py | 25 +++++ src/unitdev02/unitdev02/test01.md | 34 ++++++- src/unitdev02/unitdev02/test01.py | 57 +++++++++-- 10 files changed, 245 insertions(+), 10 deletions(-) create mode 100644 src/fc_network_adapter/fc_network_adapter/__init__.py create mode 100644 src/fc_network_adapter/fc_network_adapter/mavlinkObject.py create mode 100644 src/fc_network_adapter/fc_network_adapter/socketManager.py create mode 100644 src/fc_network_adapter/package.xml create mode 100644 src/fc_network_adapter/resource/fc_network_adapter create mode 100644 src/fc_network_adapter/setup.cfg create mode 100644 src/fc_network_adapter/setup.py diff --git a/README.md b/README.md index c93bd79..b92299c 100644 --- a/README.md +++ b/README.md @@ -16,5 +16,11 @@ === Package 簡述 +1. unitdev 為各自協作者做開發時的測試區 +2. fc_network_adapter + 建立、維護與飛控韌體的連接 + 構築 mavlink 封包 + 同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 + 處理無線模組的通訊格式 (XBee) diff --git a/src/fc_network_adapter/fc_network_adapter/__init__.py b/src/fc_network_adapter/fc_network_adapter/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py new file mode 100644 index 0000000..093e8ac --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -0,0 +1,97 @@ +import threading +from pymavlink import mavutil + +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +import time + +''' + +# 不同的匯流排只有單一種通訊協定 +# 匯流排接到訊息後透過 queue stack 傳送到分析器 +# 會有三種 Queue 分別作為 1. 固定串流分析器 2. 回傳封包處理器 3. 轉格式(例如 mavlink to FDM)的轉換器 +# 第三種比較麻煩 會需要用 list 管理多個轉換器的 queue +# 分析器會將解析得到的結論 再透過 ros2 的 publisher 發送出去 + +# 關於 mavlink 採用 pymavlink 這個套件 製作匯流排 +# pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面 +# 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function + +''' + +class MavlinkObject(): + def __init__(self, mavlink_socket): + self.mavlink_socket = mavlink_socket + self.msg_count = {} + # self.multiplexingDict = {} + self.multiplexingToAnalysis = [] + self.multiplexingToReturn = [] + self.multiplexingToConvert = [] + def __del__(self): + self.mavlink_socket.close() + self.stop() + + def run(self): + self.thread = threading.Thread(target=self._run_thread) + self.running = True + self.thread.start() + def stop(self): + self.running = False + + def _run_thread(self): + start_time = time.time() + 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(mavlinkMsg) + self.running = False + break + + # 每秒中 統計一次 收到的訊息總量 + elapsed_time = time.time() - start_time + if elapsed_time > 1: + print('Messages Type received (in about 1 sec) :') + print(self.msg_count) + start_time = time.time() + self.msg_count = {} + + def count_mavlink_type(self, mavlinkMsg): + msg_type = mavlinkMsg.get_type() + if msg_type in self.msg_count: + self.msg_count[msg_type] += 1 + else: + self.msg_count[msg_type] = 1 + + def publish_mavlink_data(self, mavlinkMsg): + msg = String() + msg.data = str(mavlinkMsg) + self.publisher_.publish(msg) + + + +def main(args=None): + + connection_string="udp:127.0.0.1:14550" + mavlink_socket = mavutil.mavlink_connection(connection_string) + + mavlink_object = MavlinkObject(mavlink_socket) + mavlink_object.run() + + time.sleep(5) + mavlink_object.running = False + mavlink_object.thread.join() + print('End of Program') + + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/socketManager.py b/src/fc_network_adapter/fc_network_adapter/socketManager.py new file mode 100644 index 0000000..52e8681 --- /dev/null +++ b/src/fc_network_adapter/fc_network_adapter/socketManager.py @@ -0,0 +1,14 @@ + +''' + +透過某個地方 得到 udp 或 uart 接口 +對於每個接口 視為一個獨立的物件 + +物件對於不同的接口 是為不同類型的物件 + +每個類型的物件 創建一個獨立的執行緒 來處理資料 +關於執行緒的實作 是寫在另一個模組 + +物件之間 也可以做資料轉換/轉拋 + +''' \ No newline at end of file diff --git a/src/fc_network_adapter/package.xml b/src/fc_network_adapter/package.xml new file mode 100644 index 0000000..7f65623 --- /dev/null +++ b/src/fc_network_adapter/package.xml @@ -0,0 +1,18 @@ + + + + fc_network_adapter + 0.0.0 + TODO: Package description + picars + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/fc_network_adapter/resource/fc_network_adapter b/src/fc_network_adapter/resource/fc_network_adapter new file mode 100644 index 0000000..e69de29 diff --git a/src/fc_network_adapter/setup.cfg b/src/fc_network_adapter/setup.cfg new file mode 100644 index 0000000..d4fbae2 --- /dev/null +++ b/src/fc_network_adapter/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fc_network_adapter +[install] +install_scripts=$base/lib/fc_network_adapter diff --git a/src/fc_network_adapter/setup.py b/src/fc_network_adapter/setup.py new file mode 100644 index 0000000..33414cb --- /dev/null +++ b/src/fc_network_adapter/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'fc_network_adapter' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='picars', + maintainer_email='chiyu1468@hotmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/unitdev02/unitdev02/test01.md b/src/unitdev02/unitdev02/test01.md index d265f70..053d1ac 100644 --- a/src/unitdev02/unitdev02/test01.md +++ b/src/unitdev02/unitdev02/test01.md @@ -46,4 +46,36 @@ RC_CHANNELS：遙控器 (RC) 的通道數據，代表從操控者 (或模擬器) 'MISSION_CURRENT': 73, 'SIMSTATE': 72, 7. 任務與導航 MISSION_CURRENT：目前正在執行的飛行任務 (Mission) 的編號。 -SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。 \ No newline at end of file +SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。 + + + +================================ + + +['__annotations__', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__getitem__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', + +'_crc', '_fieldnames', '_header', '_instance_field', '_instance_offset', '_instances', '_link_id', '_msgbuf', '_pack', '_payload', '_posted', '_signed', '_timestamp', , + +, 'crc_extra', 'fieldunits_by_name', 'format_attr', 'get_crc', , 'get_header', 'get_link_id', , 'get_msgbuf', 'get_payload', 'get_seq', 'get_signed', 'instance_field', 'instance_offset', 'lengths', 'name', 'native_format', 'ordered_fieldnames', 'orders', 'pack', 'pitch', 'pitchspeed', 'roll', 'rollspeed', 'sign_packet', 'time_boot_ms', 'to_dict', 'to_json', 'unpacker', 'yaw', 'yawspeed'] + +mavlink 封包 種類名稱 +'get_type()' '_type' 'msgname' +'get_msgId()' 'id' +'fieldenums_by_name' + + +發送端的 sysid get_srcComponent() +發送端的 compid get_srcComponent() + +這邊是一組的 mavlink 封包 內容的 +資料長度 'array_lengths' +資料名稱 'fieldnames' 'get_fieldnames()' +資料格式 'fieldtypes' + +===================================== + + +['_MAVLink__callbacks', '_MAVLink__parse_char_legacy', '__class__', '__delattr__', '__dict__', '__dir__', '__doc__', '__eq__', '__format__', '__ge__', '__getattribute__', '__gt__', '__hash__', '__init__', '__init_subclass__', '__le__', '__lt__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', 'actuator_control_target_encode', 'actuator_control_target_send', 'actuator_output_status_encode', 'actuator_output_status_send', 'adsb_vehicle_encode', 'adsb_vehicle_send', 'ais_vessel_encode', 'ais_vessel_send', 'altitude_encode', 'altitude_send', 'att_pos_mocap_encode', 'att_pos_mocap_send', 'attitude_encode', 'attitude_quaternion_cov_encode', 'attitude_quaternion_cov_send', 'attitude_quaternion_encode', 'attitude_quaternion_send', 'attitude_send', 'attitude_target_encode', 'attitude_target_send', 'auth_key_encode', 'auth_key_send', 'autopilot_state_for_gimbal_device_encode', 'autopilot_state_for_gimbal_device_send', 'autopilot_version_encode', 'autopilot_version_send', 'battery_status_encode', 'battery_status_send', 'buf', 'buf_index', 'buf_len', 'button_change_encode', 'button_change_send', 'bytes_needed', 'callback', 'callback_args', 'callback_kwargs', 'camera_capture_status_encode', 'camera_capture_status_send', 'camera_fov_status_encode', 'camera_fov_status_send', 'camera_image_captured_encode', 'camera_image_captured_send', 'camera_information_encode', 'camera_information_send', 'camera_settings_encode', 'camera_settings_send', 'camera_thermal_range_encode', 'camera_thermal_range_send', 'camera_tracking_geo_status_encode', 'camera_tracking_geo_status_send', 'camera_tracking_image_status_encode', 'camera_tracking_image_status_send', 'camera_trigger_encode', 'camera_trigger_send', 'can_filter_modify_encode', 'can_filter_modify_send', 'can_frame_encode', 'can_frame_send', 'canfd_frame_encode', 'canfd_frame_send', 'change_operator_control_ack_encode', 'change_operator_control_ack_send', 'change_operator_control_encode', 'change_operator_control_send', 'check_signature', 'collision_encode', 'collision_send', 'command_ack_encode', 'command_ack_send', 'command_int_encode', 'command_int_send', 'command_long_encode', 'command_long_send', 'control_system_state_encode', 'control_system_state_send', 'crc_extra', 'data_stream_encode', 'data_stream_send', 'data_transmission_handshake_encode', 'data_transmission_handshake_send', 'debug_encode', 'debug_float_array_encode', 'debug_float_array_send', 'debug_send', 'debug_vect_encode', 'debug_vect_send', 'decode', 'distance_sensor_encode', 'distance_sensor_send', 'efi_status_encode', 'efi_status_send', 'encapsulated_data_encode', 'encapsulated_data_send', 'estimator_status_encode', 'estimator_status_send', 'expected_length', 'extended_sys_state_encode', 'extended_sys_state_send', 'fence_status_encode', 'fence_status_send', 'file', 'file_transfer_protocol_encode', 'file_transfer_protocol_send', 'flight_information_encode', 'flight_information_send', 'follow_target_encode', 'follow_target_send', 'generator_status_encode', 'generator_status_send', 'gimbal_device_attitude_status_encode', 'gimbal_device_attitude_status_send', 'gimbal_device_information_encode', 'gimbal_device_information_send', 'gimbal_device_set_attitude_encode', 'gimbal_device_set_attitude_send', 'gimbal_manager_information_encode', 'gimbal_manager_information_send', 'gimbal_manager_set_attitude_encode', 'gimbal_manager_set_attitude_send', 'gimbal_manager_set_manual_control_encode', 'gimbal_manager_set_manual_control_send', 'gimbal_manager_set_pitchyaw_encode', 'gimbal_manager_set_pitchyaw_send', 'gimbal_manager_status_encode', 'gimbal_manager_status_send', 'global_position_int_cov_encode', 'global_position_int_cov_send', 'global_position_int_encode', 'global_position_int_send', 'global_vision_position_estimate_encode', 'global_vision_position_estimate_send', 'gps2_raw_encode', 'gps2_raw_send', 'gps2_rtk_encode', 'gps2_rtk_send', 'gps_global_origin_encode', 'gps_global_origin_send', 'gps_inject_data_encode', 'gps_inject_data_send', 'gps_input_encode', 'gps_input_send', 'gps_raw_int_encode', 'gps_raw_int_send', 'gps_rtcm_data_encode', 'gps_rtcm_data_send', 'gps_rtk_encode', 'gps_rtk_send', 'gps_status_encode', 'gps_status_send', 'have_prefix_error', 'heartbeat_encode', 'heartbeat_send', 'high_latency2_encode', 'high_latency2_send', 'high_latency_encode', 'high_latency_send', 'highres_imu_encode', 'highres_imu_send', 'hil_actuator_controls_encode', 'hil_actuator_controls_send', 'hil_controls_encode', 'hil_controls_send', 'hil_gps_encode', 'hil_gps_send', 'hil_optical_flow_encode', 'hil_optical_flow_send', 'hil_rc_inputs_raw_encode', 'hil_rc_inputs_raw_send', 'hil_sensor_encode', 'hil_sensor_send', 'hil_state_encode', 'hil_state_quaternion_encode', 'hil_state_quaternion_send', 'hil_state_send', 'home_position_encode', 'home_position_send', 'hygrometer_sensor_encode', 'hygrometer_sensor_send', 'isbd_link_status_encode', 'isbd_link_status_send', 'landing_target_encode', 'landing_target_send', 'little_endian', 'local_position_ned_cov_encode', 'local_position_ned_cov_send', 'local_position_ned_encode', 'local_position_ned_send', 'local_position_ned_system_global_offset_encode', 'local_position_ned_system_global_offset_send', 'log_data_encode', 'log_data_send', 'log_entry_encode', 'log_entry_send', 'log_erase_encode', 'log_erase_send', 'log_request_data_encode', 'log_request_data_send', 'log_request_end_encode', 'log_request_end_send', 'log_request_list_encode', 'log_request_list_send', 'logging_ack_encode', 'logging_ack_send', 'logging_data_acked_encode', 'logging_data_acked_send', 'logging_data_encode', 'logging_data_send', 'mag_cal_report_encode', 'mag_cal_report_send', 'manual_control_encode', 'manual_control_send', 'manual_setpoint_encode', 'manual_setpoint_send', 'mav10_unpacker', 'mav20_h3_unpacker', 'mav20_unpacker', 'mav_csum_unpacker', 'mav_sign_unpacker', 'memory_vect_encode', 'memory_vect_send', 'message_interval_encode', 'message_interval_send', 'mission_ack_encode', 'mission_ack_send', 'mission_clear_all_encode', 'mission_clear_all_send', 'mission_count_encode', 'mission_count_send', 'mission_current_encode', 'mission_current_send', 'mission_item_encode', 'mission_item_int_encode', 'mission_item_int_send', 'mission_item_reached_encode', 'mission_item_reached_send', 'mission_item_send', 'mission_request_encode', 'mission_request_int_encode', 'mission_request_int_send', 'mission_request_list_encode', 'mission_request_list_send', 'mission_request_partial_list_encode', 'mission_request_partial_list_send', 'mission_request_send', 'mission_set_current_encode', 'mission_set_current_send', 'mission_write_partial_list_encode', 'mission_write_partial_list_send', 'mount_orientation_encode', 'mount_orientation_send', 'named_value_float_encode', 'named_value_float_send', 'named_value_int_encode', 'named_value_int_send', 'nav_controller_output_encode', 'nav_controller_output_send', 'obstacle_distance_encode', 'obstacle_distance_send', 'odometry_encode', 'odometry_send', 'open_drone_id_arm_status_encode', 'open_drone_id_arm_status_send', 'open_drone_id_authentication_encode', 'open_drone_id_authentication_send', 'open_drone_id_basic_id_encode', 'open_drone_id_basic_id_send', 'open_drone_id_location_encode', 'open_drone_id_location_send', 'open_drone_id_message_pack_encode', 'open_drone_id_message_pack_send', 'open_drone_id_operator_id_encode', 'open_drone_id_operator_id_send', 'open_drone_id_self_id_encode', 'open_drone_id_self_id_send', 'open_drone_id_system_encode', 'open_drone_id_system_send', 'open_drone_id_system_update_encode', 'open_drone_id_system_update_send', 'optical_flow_encode', 'optical_flow_rad_encode', 'optical_flow_rad_send', 'optical_flow_send', 'param_ext_ack_encode', 'param_ext_ack_send', 'param_ext_request_list_encode', 'param_ext_request_list_send', 'param_ext_request_read_encode', 'param_ext_request_read_send', 'param_ext_set_encode', 'param_ext_set_send', 'param_ext_value_encode', 'param_ext_value_send', 'param_map_rc_encode', 'param_map_rc_send', 'param_request_list_encode', 'param_request_list_send', 'param_request_read_encode', 'param_request_read_send', 'param_set_encode', 'param_set_send', 'param_value_encode', 'param_value_send', 'parse_buffer', 'parse_char', 'ping_encode', 'ping_send', 'play_tune_encode', 'play_tune_send', 'position_target_global_int_encode', 'position_target_global_int_send', 'position_target_local_ned_encode', 'position_target_local_ned_send', 'power_status_encode', 'power_status_send', 'protocol_marker', 'radio_status_encode', 'radio_status_send', 'raw_imu_encode', 'raw_imu_send', 'raw_pressure_encode', 'raw_pressure_send', 'raw_rpm_encode', 'raw_rpm_send', 'rc_channels_encode', 'rc_channels_override_encode', 'rc_channels_override_send', 'rc_channels_raw_encode', 'rc_channels_raw_send', 'rc_channels_scaled_encode', 'rc_channels_scaled_send', 'rc_channels_send', 'relay_status_encode', 'relay_status_send', 'request_data_stream_encode', 'request_data_stream_send', 'resource_request_encode', 'resource_request_send', 'robust_parsing', 'safety_allowed_area_encode', 'safety_allowed_area_send', 'safety_set_allowed_area_encode', 'safety_set_allowed_area_send', 'scaled_imu2_encode', 'scaled_imu2_send', 'scaled_imu3_encode', 'scaled_imu3_send', 'scaled_imu_encode', 'scaled_imu_send', 'scaled_pressure2_encode', 'scaled_pressure2_send', 'scaled_pressure3_encode', 'scaled_pressure3_send', 'scaled_pressure_encode', 'scaled_pressure_send', 'send', 'send_callback', 'send_callback_args', 'send_callback_kwargs', 'seq', 'serial_control_encode', 'serial_control_send', 'servo_output_raw_encode', 'servo_output_raw_send', 'set_actuator_control_target_encode', 'set_actuator_control_target_send', 'set_attitude_target_encode', 'set_attitude_target_send', 'set_callback', 'set_gps_global_origin_encode', 'set_gps_global_origin_send', 'set_home_position_encode', 'set_home_position_send', 'set_mode_encode', 'set_mode_send', 'set_position_target_global_int_encode', 'set_position_target_global_int_send', 'set_position_target_local_ned_encode', 'set_position_target_local_ned_send', 'set_send_callback', 'setup_signing_encode', 'setup_signing_send', 'signing', 'sim_state_encode', 'sim_state_send', 'smart_battery_info_encode', 'smart_battery_info_send', 'sort_fields', 'srcComponent', 'srcSystem', 'startup_time', 'statustext_encode', 'statustext_send', 'storage_information_encode', 'storage_information_send', 'sys_status_encode', 'sys_status_send', 'system_time_encode', 'system_time_send', 'terrain_check_encode', 'terrain_check_send', 'terrain_data_encode', 'terrain_data_send', 'terrain_report_encode', 'terrain_report_send', 'terrain_request_encode', 'terrain_request_send', 'timesync_encode', 'timesync_send', 'total_bytes_received', 'total_bytes_sent', 'total_packets_received', 'total_packets_sent', 'total_receive_errors', 'trajectory_representation_bezier_encode', 'trajectory_representation_bezier_send', 'trajectory_representation_waypoints_encode', 'trajectory_representation_waypoints_send', 'tunnel_encode', 'tunnel_send', 'uavcan_node_info_encode', 'uavcan_node_info_send', 'uavcan_node_status_encode', 'uavcan_node_status_send', 'utm_global_position_encode', 'utm_global_position_send', 'v2_extension_encode', 'v2_extension_send', 'vfr_hud_encode', 'vfr_hud_send', 'vibration_encode', 'vibration_send', 'vicon_position_estimate_encode', 'vicon_position_estimate_send', 'video_stream_information_encode', 'video_stream_information_send', 'video_stream_status_encode', 'video_stream_status_send', 'vision_position_estimate_encode', 'vision_position_estimate_send', 'vision_speed_estimate_encode', 'vision_speed_estimate_send', 'wheel_distance_encode', 'wheel_distance_send', 'wifi_config_ap_encode', 'wifi_config_ap_send', 'winch_status_encode', 'winch_status_send', 'wind_cov_encode', 'wind_cov_send'] + + diff --git a/src/unitdev02/unitdev02/test01.py b/src/unitdev02/unitdev02/test01.py index 7e1f96c..bf634e2 100644 --- a/src/unitdev02/unitdev02/test01.py +++ b/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() \ No newline at end of file +# fdm_parser_example() +# mavlink_analyzer_simple(8) +mavlink_btye_generator_test()