forked from chiyu1468/AirTrapMine
dev test for some mavlink command
This commit is contained in:
@@ -0,0 +1,18 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>unitdev02</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="chiyu1468@hotmail.com">picars</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/unitdev02
|
||||
[install]
|
||||
install_scripts=$base/lib/unitdev02
|
||||
@@ -0,0 +1,27 @@
|
||||
from setuptools import find_packages, setup
|
||||
|
||||
package_name = 'unitdev02'
|
||||
|
||||
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': [
|
||||
'showmavsimple = unitdev02.test01:mavlink_analyzer_simple',
|
||||
'fdm_switch_one = unitdev02.test01:fdm_swicth_example_one',
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_copyright.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
# Remove the `skip` decorator once the source file(s) have a copyright header
|
||||
@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
|
||||
@pytest.mark.copyright
|
||||
@pytest.mark.linter
|
||||
def test_copyright():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found errors'
|
||||
@@ -0,0 +1,25 @@
|
||||
# Copyright 2017 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_flake8.main import main_with_errors
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.flake8
|
||||
@pytest.mark.linter
|
||||
def test_flake8():
|
||||
rc, errors = main_with_errors(argv=[])
|
||||
assert rc == 0, \
|
||||
'Found %d code style errors / warnings:\n' % len(errors) + \
|
||||
'\n'.join(errors)
|
||||
@@ -0,0 +1,23 @@
|
||||
# Copyright 2015 Open Source Robotics Foundation, Inc.
|
||||
#
|
||||
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||
# you may not use this file except in compliance with the License.
|
||||
# You may obtain a copy of the License at
|
||||
#
|
||||
# http://www.apache.org/licenses/LICENSE-2.0
|
||||
#
|
||||
# Unless required by applicable law or agreed to in writing, software
|
||||
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
from ament_pep257.main import main
|
||||
import pytest
|
||||
|
||||
|
||||
@pytest.mark.linter
|
||||
@pytest.mark.pep257
|
||||
def test_pep257():
|
||||
rc = main(argv=['.', 'test'])
|
||||
assert rc == 0, 'Found code style errors / warnings'
|
||||
@@ -0,0 +1,49 @@
|
||||
|
||||
|
||||
'ATTITUDE': 73, 'GLOBAL_POSITION_INT': 73,'NAV_CONTROLLER_OUTPUT': 73, 'AHRS': 73, 'AHRS2': 72,'LOCAL_POSITION_NED': 72,
|
||||
1. 飛行狀態與姿態相關
|
||||
ATTITUDE：無人機的姿態資訊，包括滾轉角 (Roll)、俯仰角 (Pitch)、偏航角 (Yaw) 及其角速度。
|
||||
GLOBAL_POSITION_INT：全球位置資訊，提供經度、緯度、高度，以及無人機的相對高度 (relative altitude)。
|
||||
LOCAL_POSITION_NED：無人機在當前本地座標系統 (NED - North, East, Down) 中的位置與速度資訊。
|
||||
NAV_CONTROLLER_OUTPUT：導航控制輸出，包括航向 (heading)、導航誤差等資訊，用於導航系統內部運算。
|
||||
AHRS：輔助導航狀態，如 AHRS (Attitude and Heading Reference System) 計算的誤差、修正參數。
|
||||
AHRS2：進一步的 AHRS 擴充資訊，例如不同座標系之間的轉換參數。
|
||||
|
||||
'VFR_HUD': 73, 'RAW_IMU': 73, 'SCALED_IMU2': 73, 'SCALED_IMU3': 73, 'SCALED_PRESSURE': 73, 'SCALED_PRESSURE2': 73,'VIBRATION': 72,
|
||||
2. 感測器資訊
|
||||
RAW_IMU：IMU 原始數據，包括加速度計 (Accelerometer)、陀螺儀 (Gyroscope)、磁力計 (Magnetometer) 的測量值。
|
||||
SCALED_IMU2 / SCALED_IMU3：經過校正的 IMU 數據，可能來自第二組或第三組 IMU，提供更精確的測量值。
|
||||
SCALED_PRESSURE / SCALED_PRESSURE2：經過校正的大氣壓力感測器數據，用於高度計的計算。
|
||||
VFR_HUD：HUD (Head-Up Display) 相關資訊，如空速 (airspeed)、高度 (altitude)、油門 (throttle) 位置、飛行模式等。
|
||||
VIBRATION：機體振動資訊，包含加速度變化率等，幫助監測飛機是否有異常振動。
|
||||
|
||||
'SYS_STATUS': 73, 'POWER_STATUS': 73,'MEMINFO': 73, 'BATTERY_STATUS': 72, 'ESC_TELEMETRY_1_TO_4': 72,
|
||||
3. 動力與系統狀態
|
||||
SYS_STATUS：無人機的系統狀態，包括 CPU 負載、感測器狀態、儲存空間、記憶體狀態等。
|
||||
POWER_STATUS：無人機的電源狀態，如電壓、電流消耗、備用電池狀態等。
|
||||
BATTERY_STATUS：詳細的電池資訊，如電壓、電流、充電狀態、剩餘電量等。
|
||||
ESC_TELEMETRY_1_TO_4：電子調速器 (ESC) 的遙測數據，可能包括轉速、電流消耗、溫度等。
|
||||
MEMINFO：記憶體資訊，如剩餘 RAM、堆疊使用量等，用於監測飛控是否有記憶體不足問題。
|
||||
|
||||
'TERRAIN_REPORT': 72,'EKF_STATUS_REPORT': 72, 'GPS_RAW_INT': 73, 'WIND': 72,
|
||||
4. 導航與 GPS 相關
|
||||
GPS_RAW_INT：GPS 原始數據，如衛星數量、經緯度、高度、水平與垂直精度等資訊。
|
||||
TERRAIN_REPORT：地形資訊，可能包含當前無人機下方的地表高度 (Terrain Altitude) 等。
|
||||
EKF_STATUS_REPORT：EKF (Extended Kalman Filter) 狀態，提供導航估測誤差、狀態收斂情況等資訊。
|
||||
WIND：風速與風向資訊，用於導航與飛控補償。
|
||||
|
||||
'SYSTEM_TIME': 73, 'HEARTBEAT': 18, 'TIMESYNC': 2
|
||||
5. 通訊與時間同步
|
||||
HEARTBEAT：心跳訊號，告知系統無人機仍然運作中，並提供當前模式、系統狀態等基本資訊。
|
||||
TIMESYNC：時間同步封包，確保地面站與無人機之間的時鐘同步。
|
||||
SYSTEM_TIME：系統時間資訊，用於確保所有設備的時間基準一致。
|
||||
|
||||
'SERVO_OUTPUT_RAW': 73, 'RC_CHANNELS': 73,
|
||||
6. 控制與遙控輸入
|
||||
SERVO_OUTPUT_RAW：各個伺服馬達的 PWM (Pulse Width Modulation) 輸出數據，反映控制器實際輸出的命令。
|
||||
RC_CHANNELS：遙控器 (RC) 的通道數據，代表從操控者 (或模擬器) 端傳遞過來的命令。
|
||||
|
||||
'MISSION_CURRENT': 73, 'SIMSTATE': 72,
|
||||
7. 任務與導航
|
||||
MISSION_CURRENT：目前正在執行的飛行任務 (Mission) 的編號。
|
||||
SIMSTATE：SITL 模擬器的狀態，如飛行模式、地面速度等。
|
||||
@@ -0,0 +1,146 @@
|
||||
import rclpy
|
||||
from pymavlink import mavutil
|
||||
from time import sleep
|
||||
import time
|
||||
|
||||
import socket
|
||||
import struct
|
||||
|
||||
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)
|
||||
|
||||
else:
|
||||
print('No message yet, 1 second for data to fill')
|
||||
sleep(1)
|
||||
|
||||
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}")
|
||||
packet_count = 0
|
||||
start_time = current_time
|
||||
fdm_parser_example(data_s2g)
|
||||
|
||||
|
||||
def fdm_switch_example_two():
|
||||
'''
|
||||
這個範例在 SITL 與 Gazebo 之間 做一個簡單的UDP轉發器
|
||||
有轉換數據格式
|
||||
'''
|
||||
# read info from SITL via MAVLink
|
||||
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)
|
||||
|
||||
|
||||
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
|
||||
# )
|
||||
Reference in New Issue
Block a user