|
|
import rclpy
|
|
|
from pymavlink import mavutil
|
|
|
from time import sleep
|
|
|
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')
|
|
|
|
|
|
print('Inintializing connection')
|
|
|
|
|
|
connection_string="udp:127.0.0.1:14550"
|
|
|
connection = mavutil.mavlink_connection(connection_string)
|
|
|
|
|
|
print('Catch messages')
|
|
|
msg_count = {}
|
|
|
|
|
|
start_time = time.time()
|
|
|
|
|
|
for _ in range(count):
|
|
|
msg = connection.recv_msg()
|
|
|
# msg = connection.recv_match(blocking=True)
|
|
|
if msg:
|
|
|
msg_type = msg.get_type()
|
|
|
if msg_type in msg_count:
|
|
|
msg_count[msg_type] += 1
|
|
|
else:
|
|
|
msg_count[msg_type] = 1
|
|
|
|
|
|
if msg.get_type() == 'SERVO_OUTPUT_RAW':
|
|
|
print(msg)
|
|
|
pass
|
|
|
|
|
|
else:
|
|
|
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)
|
|
|
|
|
|
end_time = time.time()
|
|
|
print('Time elapsed: ', end_time - start_time)
|
|
|
|
|
|
print('Closing connection')
|
|
|
connection.close()
|
|
|
|
|
|
|
|
|
def fdm_parser_example(data=None):
|
|
|
if data is None:
|
|
|
data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
|
|
|
# 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
|
|
|
|
|
|
parse_format = 'HHI16H'
|
|
|
|
|
|
if len(data) != struct.calcsize(parse_format):
|
|
|
print("got packet of len %u, expected %u" % (len(data), struct.calcsize(parse_format)))
|
|
|
exit(1)
|
|
|
|
|
|
decoded = struct.unpack(parse_format,data)
|
|
|
|
|
|
magic = decoded[0]
|
|
|
frame_rate_hz = decoded[1]
|
|
|
frame_count = decoded[2]
|
|
|
pwm = decoded[3:]
|
|
|
|
|
|
|
|
|
print("magic: %u, frame_rate_hz: %u, frame_count: %u, pwm: %s" % (magic, frame_rate_hz, frame_count, pwm))
|
|
|
|
|
|
def fdm_switch_example_one():
|
|
|
|
|
|
'''
|
|
|
這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
|
|
|
沒有轉換數據格式
|
|
|
'''
|
|
|
# make a link get fdm from SITL, as a server
|
|
|
sock_s2g = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
|
sock_s2g.bind(('', 9002))
|
|
|
sock_s2g.settimeout(0.1)
|
|
|
|
|
|
# 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)
|
|
|
|
|
|
packet_count = 0
|
|
|
start_time = time.time()
|
|
|
|
|
|
while True:
|
|
|
try:
|
|
|
# 接到SITL的數據 並轉發給Gazebo
|
|
|
data_s2g, addr_s2g = sock_s2g.recvfrom(1024)
|
|
|
sock_g2s.sendto(data_s2g, server_address)
|
|
|
# 得到Gazebo的回應 並轉發給SITL
|
|
|
data_g2s, addr_g2s = sock_g2s.recvfrom(1024)
|
|
|
sock_s2g.sendto(data_g2s, addr_s2g)
|
|
|
packet_count += 1
|
|
|
except socket.timeout:
|
|
|
pass
|
|
|
|
|
|
current_time = time.time()
|
|
|
elapsed_time = current_time - start_time
|
|
|
|
|
|
if elapsed_time >= 1.0:
|
|
|
# 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)
|
|
|
|
|
|
|
|
|
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 = 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 # [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=1, frame_count_fdm=1):
|
|
|
interval = 1.0 / frame_rate_fdm
|
|
|
while True:
|
|
|
start_time = time.time()
|
|
|
try:
|
|
|
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_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
|
|
|
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()
|
|
|
print('Start sending data to Gazebo')
|
|
|
|
|
|
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)
|
|
|
|
|
|
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
|
|
|
)
|
|
|
msg = mav.command_long_encode(
|
|
|
target_system=1,
|
|
|
target_component=1,
|
|
|
command=mavutil.mavlink.MAV_CMD_DO_SET_MODE,
|
|
|
confirmation=0,
|
|
|
param1=0, # Custom mode
|
|
|
param2=0, # Custom sub-mode
|
|
|
param3=0,
|
|
|
param4=0,
|
|
|
param5=0,
|
|
|
param6=0,
|
|
|
param7=0
|
|
|
)
|
|
|
|
|
|
|
|
|
|
|
|
# 取得 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)
|
|
|
|
|
|
def simple_getMavlink():
|
|
|
connection_string="udp:127.0.0.1:14550"
|
|
|
# connection_string='/dev/ttyUSB0'
|
|
|
connection = mavutil.mavlink_connection(connection_string)
|
|
|
|
|
|
while True:
|
|
|
msg = connection.recv_msg()
|
|
|
if msg:
|
|
|
print(msg)
|
|
|
else:
|
|
|
print('No message yet, 0.1 second for data to fill')
|
|
|
sleep(0.1)
|
|
|
|
|
|
|
|
|
|
|
|
print('Start')
|
|
|
# fdm_switch_example_two()
|
|
|
# fdm_parser_example()
|
|
|
# mavlink_analyzer_simple(8)
|
|
|
# mavlink_btye_generator_test()
|
|
|
simple_getMavlink()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200)
|