diff --git a/.gitignore b/.gitignore index f247176..789d139 100644 --- a/.gitignore +++ b/.gitignore @@ -25,12 +25,4 @@ Makefile **/*.class **/*.pyc **/*.pyo -logs/ -src/logs/ - -# 個人環境筆記 -my_env_notes.md - -# Claude Code 本地設定 -CLAUDE.md -.claude/ +**/.cursor/ diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..1742bc8 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,7 @@ +[submodule "src/external/angles"] + path = src/external/angles + url = https://github.com/ros/angles.git +[submodule "src/external/geographic_info"] + path = src/external/geographic_info + url = https://github.com/ros-geographic-info/geographic_info.git + branch = ros2 diff --git a/README.md b/README.md index 26d7903..0cbf700 100644 --- a/README.md +++ b/README.md @@ -7,22 +7,51 @@ === -必要相依套件 +必要相依套件 順便記錄我開發時的環境版本 Python -1. pymavlink +1. pymavlink -> Version: 2.4.42 2. conda-forge 中的 pyserial-asyncio -3. +3. importlib_metadata -> Version: 8.5.0 +4. setuptools -> Version: 58.2.0 (版本太新不行) +5. pyserial-asyncio + ROS2 1. source ~/ros2_humble/install/setup.bash 2. +=== +功能簡介 +1. mavlink 多對多支援平台 +2. 不允許進到 ros 系統有相同 sysid +3. 假設所有 component 共用同一 socket + === 開發用輔助專案 1. Gazebo Garden 2. Ardupilot +=== +依賴的 ROS 庫 +1. https://github.com/ros-geographic-info/geographic_info.git 記得要搞 ros2 版本的 +2. https://github.com/ros/angles.git +3. mavros_msgs 是 https://github.com/mavlink/mavros 這個專案中的一個資料夾 這邊手動複製的 + +Clone 專案後 請先執行這些指令 +```bash +# 1.同步 submodule +cd ~/AirTrapMine +git submodule init +git submodule update +# 2. build 需要的 package +colcon build --packages-select angles geographic_msgs +colcon build --packages-select mavros_msgs # 這個依賴前面的 +colcon build --packages-select fc_interfaces # 自己定義的 + +``` + + === Package 簡述 <<<<<<< HEAD @@ -39,7 +68,26 @@ Package 簡述 2. fc_network_adapter 建立、維護與飛控韌體的連接 構築 mavlink 封包 - 同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 處理無線模組的通訊格式 (XBee) + --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- +3. fc_interfaces + 自定義的介面檔 +4. fc_network_apps + 與 fc_network_adapter 銜接做高階功能包裝的應用小程式 利於開發GUI或其他應用 + +N. logs 是執行時期的記錄檔 + +=== +主要專案 fc_network_adapter 請一律在 ~/AirTrapMine/src/ 資料夾下 以模組化啟動程式 + +例如 在 ~/AirTrapMine/src/ 資料夾下 +```bash +# 記得先開啟 依賴 Package 到 overlay +. ./install/local_setup.bash +# 範例 +python -m fc_network_adapter.fc_network_adapter.mainOrchestrator +python -m fc_network_adapter.tests.demo_integration + +``` >>>>>>> chiyu diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 9e82fbc..6e9ffcc 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -113,12 +113,15 @@ class UDPMavlinkReceiver(threading.Thread): }) elif msg_type == "ATTITUDE": + # 從 MAVLink 訊息中提取並轉為角度 pitch = math.degrees(msg.pitch) + roll = math.degrees(msg.roll) + yaw = math.degrees(msg.yaw) self.signals.update_signal.emit('attitude', drone_id, { 'pitch': pitch, - 'roll': 0, - 'yaw': 0, - 'rates': (0, 0, 0) + 'roll': roll, + 'yaw': yaw, + 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed) }) elif msg_type == "VFR_HUD": @@ -244,12 +247,15 @@ class SerialMavlinkReceiver(threading.Thread): }) elif msg_type == "ATTITUDE": + # 從 MAVLink 訊息中提取並轉為角度 pitch = math.degrees(msg.pitch) + roll = math.degrees(msg.roll) + yaw = math.degrees(msg.yaw) self.signals.update_signal.emit('attitude', drone_id, { 'pitch': pitch, - 'roll': 0, - 'yaw': 0, - 'rates': (0, 0, 0) + 'roll': roll, + 'yaw': yaw, + 'rates': (msg.rollspeed, msg.pitchspeed, msg.yawspeed) }) elif msg_type == "VFR_HUD": diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py index adf142f..49e560e 100644 --- a/src/GUI/drone_panel.py +++ b/src/GUI/drone_panel.py @@ -486,7 +486,7 @@ class AttitudeIndicator(QWidget): # ---- rotate + translate canvas for roll & pitch p.save() p.translate(cx, cy) - p.rotate(self.roll) + p.rotate(-self.roll) # ✅ 負值：NED 坐標系轉換 pitch_offset = self.pitch * ppd # sky (above horizon) @@ -535,7 +535,7 @@ class AttitudeIndicator(QWidget): p.drawLine(QPointF(x1, y1), QPointF(x2, y2)) # roll pointer triangle (rotates with roll) - p.rotate(self.roll) + p.rotate(-self.roll) # ✅ 負值：NED 坐標系轉換 ptr_r = arc_r - 1 tri = QPolygonF([ QPointF(0, -ptr_r), diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 2dd8df5..a850097 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -9,6 +9,7 @@ import sys import asyncio import json import subprocess +import time # 導入分離的類別 from communication import DroneMonitor, UDPMavlinkReceiver, WebSocketMavlinkReceiver @@ -26,7 +27,7 @@ from mission_executor import MissionExecutor # ================================================================================ class ControlStationUI(QMainWindow): - VERSION = '1.0.0' + VERSION = '1.0.1' def __init__(self): super().__init__() @@ -47,6 +48,14 @@ class ControlStationUI(QMainWindow): self.timer.timeout.connect(self.spin_ros) self.timer.start(10) + # 初始化 panel 和 map 更新（10Hz） + self.panel_map_timer = QTimer() + self.panel_map_timer.timeout.connect(self._update_panel_and_map) + self.panel_map_timer.start(100) # 10Hz + + # 快取消息數據，以便在沒有新消息時使用上一次的值 + self._message_cache = {} + # 初始化UI self.drones = {} self.socket_groups = {} @@ -492,6 +501,7 @@ class ControlStationUI(QMainWindow): # ================================================================================ def update_ui(self, msg_type, drone_id, data): + """只做數據快取，不在這裡更新 UI""" if msg_type == 'connection_type': conn_type = data.get('type', 'Unknown') parts = drone_id.split('_') @@ -507,121 +517,12 @@ class ControlStationUI(QMainWindow): self.add_drone(drone_id) return - if not (panel := self.drones.get(drone_id)): - return + # 只做資料快取，不更新 UI - 所有 UI 更新都在 _update_panel_and_map 中進行 + if drone_id not in self._message_cache: + self._message_cache[drone_id] = {} + + self._message_cache[drone_id][msg_type] = data - if msg_type == 'state': - mode = data.get('mode', 'UNKNOWN') - armed = data.get('armed', None) - mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' - if armed is True: - arm_text, arm_color = "ARMED", '#55FF55' - elif armed is False: - arm_text, arm_color = "DISARMED", '#FF5555' - else: - arm_text, arm_color = "--", '#AAAAAA' - self.update_field(panel, drone_id, 'mode', mode, mode_color) - self.update_field(panel, drone_id, 'armed', arm_text, arm_color) - self.update_overview_table(drone_id, 'mode', mode) - self.update_overview_table(drone_id, 'armed', arm_text) - - elif msg_type == 'battery': - voltage = data.get('voltage', 16) - cells = round(voltage / 3.95) - percentage = (voltage / cells - 3.7) / 0.5 * 100 if cells > 0 else 0 - if percentage < 20: voltage_color = '#FF6464' - elif percentage < 50: voltage_color = '#FFA500' - else: voltage_color = '#FFFFFF' - percentage = data.get('percentage', percentage) - self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color) - self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V") - self.update_field(panel, drone_id, 'battery_cells', f"{cells}S") - self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V") - - elif msg_type == 'gps': - lat, lon = data.get('lat', 0), data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - alt = data.get('alt', 0) - if not hasattr(self.monitor, 'drone_gps'): - self.monitor.drone_gps = {} - self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt} - self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") - self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") - heading = self.drone_headings.get(drone_id, 0) - self.drone_map.update_drone_position(drone_id, lat, lon, heading) - - elif msg_type == 'altitude': - altitude = data.get('altitude', 0) - text = f"{altitude:.1f} m" - self.update_field(panel, drone_id, 'altitude', text) - self.update_overview_table(drone_id, 'altitude', text) - - elif msg_type == 'local_pose': - x, y = data.get('x', 0), data.get('y', 0) - if not hasattr(self.monitor, 'drone_local'): - self.monitor.drone_local = {} - self.monitor.drone_local[drone_id] = {'x': x, 'y': y} - self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}") - - elif msg_type == 'hud': - heading = data.get('heading') - self.drone_headings[drone_id] = heading - groundspeed = data.get('groundspeed') - airspeed = data.get('airspeed') - throttle = data.get('throttle') - hud_alt = data.get('alt') - climb = data.get('climb') - - heading_text = f"{heading:.1f}°" - groundspeed_text = f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--" - airspeed_text = f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--" - throttle_text = f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--" - hud_alt_text = f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--" - climb_text = f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--" - - self.update_field(panel, drone_id, 'heading', heading_text) - self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) - self.update_field(panel, drone_id, 'speed', groundspeed_text) - self.update_overview_table(drone_id, 'heading', heading_text) - self.update_overview_table(drone_id, 'groundspeed', groundspeed_text) - self.update_overview_table(drone_id, 'airspeed', airspeed_text) - self.update_overview_table(drone_id, 'throttle', throttle_text) - self.update_overview_table(drone_id, 'hud_alt', hud_alt_text) - self.update_overview_table(drone_id, 'climb', climb_text) - - if panel and hasattr(panel, 'attitude_indicator') and panel.attitude_indicator: - if not hasattr(panel, '_last_roll'): panel._last_roll = 0 - if not hasattr(panel, '_last_pitch'): panel._last_pitch = 0 - panel.attitude_indicator.update_attitude(heading, panel._last_roll, panel._last_pitch) - - if drone_id in self.drone_positions: - lat, lon = self.drone_positions[drone_id] - self.drone_map.update_drone_position(drone_id, lat, lon, heading) - - elif msg_type == 'loss_rate': - text = f"{data.get('loss_rate', 0):.1f}%" - self.update_field(panel, drone_id, 'loss_rate', text) - self.update_overview_table(drone_id, 'loss_rate', text) - - elif msg_type == 'ping': - text = f"{data.get('ping', 0):.1f} ms" - self.update_field(panel, drone_id, 'ping', text) - self.update_overview_table(drone_id, 'ping', text) - - elif msg_type == 'velocity': - self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") - - elif msg_type == 'attitude': - roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0) - self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°") - self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°") - self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°") - if panel: - panel._last_roll = roll - panel._last_pitch = pitch - if panel and hasattr(panel, 'update_attitude'): - heading = self.drone_headings.get(drone_id, 0) - panel.update_attitude(heading, roll, pitch) # ================================================================================ # 勾選管理 @@ -1037,6 +938,153 @@ class ControlStationUI(QMainWindow): for socket_id in sorted(self.socket_groups.keys(), key=lambda x: int(x)): self.drone_panel_layout.addWidget(self.socket_groups[socket_id]) + def _update_panel_and_map(self): + """30Hz 定時更新 panel 和 map，批量更新 UI 以避免過度重繪""" + if not hasattr(self, '_message_cache') or not self._message_cache: + return + + # 頻率監控 + if not hasattr(self, '_map_update_time'): + self._map_update_time = time.time() + self._map_update_count = 0 + + self._map_update_count += 1 + now = time.time() + if now - self._map_update_time >= 1.0: + print(f"[Panel/Map Update] {self._map_update_count} Hz") + self._map_update_time = now + self._map_update_count = 0 + + # ✅ 步驟 1: 暫停表格的即時重繪 + if hasattr(self, 'overview_table') and self.overview_table: + self.overview_table.setUpdatesEnabled(False) + + try: + start_time = time.time() + + # ✅ 步驟 2: 遍歷快取中最新的資料來更新 UI + for drone_id in list(self._message_cache.keys()): + if drone_id not in self.drones: + continue + + panel = self.drones[drone_id] + cached_data = self._message_cache[drone_id] + + # 處理所有快取的消息類型 + for msg_type, data in cached_data.items(): + if msg_type == 'state': + mode = data.get('mode', 'UNKNOWN') + armed = data.get('armed', None) + mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + if armed is True: + arm_text, arm_color = "ARMED", '#55FF55' + elif armed is False: + arm_text, arm_color = "DISARMED", '#FF5555' + else: + arm_text, arm_color = "--", '#AAAAAA' + self.update_field(panel, drone_id, 'mode', mode, mode_color) + self.update_field(panel, drone_id, 'armed', arm_text, arm_color) + self.update_overview_table(drone_id, 'mode', mode) + self.update_overview_table(drone_id, 'armed', arm_text) + + elif msg_type == 'battery': + voltage = data.get('voltage', 16) + cells = round(voltage / 3.95) + percentage = (voltage / cells - 3.7) / 0.5 * 100 if cells > 0 else 0 + if percentage < 20: voltage_color = '#FF6464' + elif percentage < 50: voltage_color = '#FFA500' + else: voltage_color = '#FFFFFF' + percentage = data.get('percentage', percentage) + self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color) + self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V") + self.update_field(panel, drone_id, 'battery_cells', f"{cells}S") + self.update_overview_table(drone_id, 'battery', f"{voltage:.2f}V") + + elif msg_type == 'altitude': + altitude = data.get('altitude', 0) + text = f"{altitude:.1f} m" + self.update_field(panel, drone_id, 'altitude', text) + self.update_overview_table(drone_id, 'altitude', text) + + elif msg_type == 'local_pose': + x, y = data.get('x', 0), data.get('y', 0) + if not hasattr(self.monitor, 'drone_local'): + self.monitor.drone_local = {} + self.monitor.drone_local[drone_id] = {'x': x, 'y': y} + self.update_overview_table(drone_id, 'local', f"{x:.1f}, {y:.1f}") + + elif msg_type == 'loss_rate': + text = f"{data.get('loss_rate', 0):.1f}%" + self.update_field(panel, drone_id, 'loss_rate', text) + self.update_overview_table(drone_id, 'loss_rate', text) + + elif msg_type == 'ping': + text = f"{data.get('ping', 0):.1f} ms" + self.update_field(panel, drone_id, 'ping', text) + self.update_overview_table(drone_id, 'ping', text) + + elif msg_type == 'velocity': + self.update_overview_table(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") + + elif msg_type == 'attitude': + roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0) + self.update_overview_table(drone_id, 'roll', f"{roll:.1f}°") + self.update_overview_table(drone_id, 'pitch', f"{pitch:.1f}°") + self.update_overview_table(drone_id, 'yaw', f"{yaw:.1f}°") + panel._last_roll = roll + panel._last_pitch = pitch + if hasattr(panel, 'update_attitude'): + heading = self.drone_headings.get(drone_id, 0) + panel.update_attitude(heading, roll, pitch) + + elif msg_type == 'gps': + gps_data = data + lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) + self.drone_positions[drone_id] = (lat, lon) + alt = gps_data.get('alt', 0) + if not hasattr(self.monitor, 'drone_gps'): + self.monitor.drone_gps = {} + self.monitor.drone_gps[drone_id] = {'lat': lat, 'lon': lon, 'alt': alt} + self.update_overview_table(drone_id, 'latitude', f"{lat:.6f}°") + self.update_overview_table(drone_id, 'longitude', f"{lon:.6f}°") + + elif msg_type == 'hud': + hud_data = data + heading = hud_data.get('heading', 0) + self.drone_headings[drone_id] = heading + groundspeed = hud_data.get('groundspeed', 0) + airspeed = hud_data.get('airspeed', 0) + throttle = hud_data.get('throttle', 0) + hud_alt = hud_data.get('alt', 0) + climb = hud_data.get('climb', 0) + + self.update_overview_table(drone_id, 'heading', f"{heading:.1f}°") + self.update_overview_table(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") + self.update_overview_table(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--") + self.update_overview_table(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--") + self.update_overview_table(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--") + self.update_overview_table(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--") + + self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°") + self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") + self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") + + if drone_id in self.drone_positions: + lat, lon = self.drone_positions[drone_id] + self.drone_map.update_drone_position(drone_id, lat, lon, heading) + + elapsed = (time.time() - start_time) * 1000 + if elapsed > 33: + print(f"[WARNING] UI update took {elapsed:.1f}ms (target: <33ms)") + + finally: + # ✅ 步驟 3: 恢復表格重繪（所有資料已填好，一次性重繪） + if hasattr(self, 'overview_table') and self.overview_table: + self.overview_table.setUpdatesEnabled(True) + self.overview_table.viewport().update() + + + def spin_ros(self): try: self.executor.spin_once(timeout_sec=0.01) diff --git a/src/GUI/validation/test_mission.py b/src/GUI/validation/test_mission.py new file mode 100644 index 0000000..22b6e22 --- /dev/null +++ b/src/GUI/validation/test_mission.py @@ -0,0 +1,299 @@ +#!/usr/bin/env python3 +""" +獨立測試腳本 — 驗證 MissionExecutor + MavlinkSender 在 SITL 環境下的運作 + +使用方式: + 1. 啟動 SITL + 2. 修改下方 CONFIG 區塊 + 3. python3 test_mission.py +""" +import sys, os +sys.path.insert(0, os.path.join(os.path.dirname(os.path.abspath(__file__)), '..')) + +import time +from pymavlink import mavutil +from PyQt6.QtWidgets import QApplication +from PyQt6.QtCore import QTimer + +from mission_planner import FormationPlanner, MissionType +from command_sender import MavlinkSender +from mission_executor import MissionExecutor, MissionState + + +# ================================================================================ +# CONFIG +# ================================================================================ + +# 接收用連線 (讀取無人機狀態) +RECV_CONNECTION = "udp:127.0.0.1:14550" + +# 發送用連線 (發送 setpoint 指令) +SEND_CONNECTION = "udpout:127.0.0.1:14550" + +# 要控制的無人機 sysid 列表 +DRONE_SYSIDS = [1] + +# 起飛高度 (公尺) +TAKEOFF_ALT = 10.0 + +# 任務規劃參數 +FORMATION_SPACING = 5.0 +BASE_ALTITUDE = 10.0 +ALTITUDE_DIFF = 2.0 +ARRIVAL_RADIUS = 2.0 + +# 測試模式: "formation" 或 "grid_sweep" +TEST_MODE = "formation" + +# Grid Sweep 專用設定 +GRID_LINE_SPACING = 5.0 + +# ================================================================================ + + +class SITLDroneManager: + """管理 SITL 無人機的連線、起飛前置作業""" + + def __init__(self, connection_string, sysids): + self.connection_string = connection_string + self.sysids = sysids + self.mav = None + self.drone_gps = {} + + def connect(self): + """建立 MAVLink 連線並等待心跳""" + print(f"連線到 {self.connection_string} ...") + self.mav = mavutil.mavlink_connection(self.connection_string) + self.mav.wait_heartbeat() + print(f"已收到心跳: sysid={self.mav.target_system}, compid={self.mav.target_component}") + + def set_guided_and_arm(self, sysid): + """切換到 GUIDED 模式並解鎖""" + print(f"\n--- sysid={sysid}: 切換 GUIDED + 解鎖 ---") + + # 切換 GUIDED 模式 + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_DO_SET_MODE, + 0, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + 4, # GUIDED = 4 + 0, 0, 0, 0, 0 + ) + time.sleep(1) + + # 解鎖 + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, + 0, 1, 0, 0, 0, 0, 0, 0 + ) + time.sleep(1) + print(f" sysid={sysid}: GUIDED + ARMED") + + def takeoff(self, sysid, altitude): + """起飛到指定高度""" + print(f" sysid={sysid}: 起飛到 {altitude}m ...") + self.mav.mav.command_long_send( + sysid, 1, + mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, + 0, 0, 0, 0, 0, 0, 0, altitude + ) + + def wait_for_altitude(self, sysid, target_alt, timeout=30): + """等待無人機到達指定高度""" + print(f" sysid={sysid}: 等待到達 {target_alt}m ...") + start = time.time() + while time.time() - start < timeout: + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if msg and msg.get_srcSystem() == sysid: + alt = msg.relative_alt / 1000.0 + if alt >= target_alt * 0.9: + print(f" sysid={sysid}: 已到達 {alt:.1f}m") + return True + print(f" sysid={sysid}: 等待超時!") + return False + + def update_gps_once(self): + """讀取一輪 GPS 資料更新 drone_gps""" + deadline = time.time() + 3 + received = set() + while time.time() < deadline and len(received) < len(self.sysids): + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) + if msg is None: + continue + sid = msg.get_srcSystem() + if sid in self.sysids: + drone_id = f"s0_{sid}" + self.drone_gps[drone_id] = { + 'lat': msg.lat / 1e7, + 'lon': msg.lon / 1e7, + 'alt': msg.relative_alt / 1000.0 + } + received.add(sid) + + for sid in self.sysids: + drone_id = f"s0_{sid}" + if drone_id in self.drone_gps: + gps = self.drone_gps[drone_id] + print(f" {drone_id}: ({gps['lat']:.6f}, {gps['lon']:.6f}, {gps['alt']:.1f}m)") + else: + print(f" {drone_id}: 尚未收到 GPS") + + def start_gps_polling(self, interval_ms=200): + """啟動定時 GPS 輪詢 (用 QTimer)""" + self._gps_timer = QTimer() + self._gps_timer.timeout.connect(self._poll_gps) + self._gps_timer.start(interval_ms) + + def _poll_gps(self): + """非阻塞方式讀取最新 GPS""" + while True: + msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=False) + if msg is None: + break + sid = msg.get_srcSystem() + if sid in self.sysids: + drone_id = f"s0_{sid}" + self.drone_gps[drone_id] = { + 'lat': msg.lat / 1e7, + 'lon': msg.lon / 1e7, + 'alt': msg.relative_alt / 1000.0 + } + + +def main(): + # 建立 Qt 應用 (MissionExecutor 需要 QTimer) + app = QApplication(sys.argv) + + # 連線 + 起飛前置作業 + manager = SITLDroneManager(RECV_CONNECTION, DRONE_SYSIDS) + manager.connect() + + for sysid in DRONE_SYSIDS: + manager.set_guided_and_arm(sysid) + manager.takeoff(sysid, TAKEOFF_ALT) + + # 等待所有無人機到達起飛高度 + for sysid in DRONE_SYSIDS: + manager.wait_for_altitude(sysid, TAKEOFF_ALT) + + time.sleep(2) + + # 讀取當前 GPS 位置 + print("\n讀取當前 GPS 位置 ...") + manager.update_gps_once() + + drone_ids = [f"s0_{sid}" for sid in DRONE_SYSIDS] + drone_gps_positions = [] + for drone_id in drone_ids: + gps = manager.drone_gps.get(drone_id) + if gps is None: + print(f"錯誤: 讀不到 {drone_id} 的 GPS") + return + drone_gps_positions.append((gps['lat'], gps['lon'], gps['alt'])) + + # 規劃任務 + print(f"\n規劃任務 (模式: {TEST_MODE}) ...") + planner = FormationPlanner( + spacing=FORMATION_SPACING, + base_altitude=BASE_ALTITUDE, + altitude_diff=ALTITUDE_DIFF + ) + + center_lat = drone_gps_positions[0][0] + center_lon = drone_gps_positions[0][1] + + if TEST_MODE == "formation": + target_lat = center_lat + 30.0 / 111000.0 + target_lon = center_lon + target_gps = (target_lat, target_lon, BASE_ALTITUDE) + print(f" 目標點: ({target_lat:.6f}, {target_lon:.6f})") + + waypoints_per_drone, origin = planner.plan_formation_mission( + drone_gps_positions, target_gps, MissionType.M_FORMATION + ) + + elif TEST_MODE == "grid_sweep": + # 在無人機前方 30m 處建立 40m x 30m 的矩形 + offset_lat = 30.0 / 111000.0 + half_w = 20.0 / (111000.0 * 0.9) + half_h = 15.0 / 111000.0 + + rect_center_lat = center_lat + offset_lat + rect_center_lon = center_lon + + rect_corners = [ + (rect_center_lat - half_h, rect_center_lon - half_w), + (rect_center_lat - half_h, rect_center_lon + half_w), + (rect_center_lat + half_h, rect_center_lon + half_w), + (rect_center_lat + half_h, rect_center_lon - half_w), + ] + target_gps = (rect_center_lat, rect_center_lon, BASE_ALTITUDE) + print(f" 矩形中心: ({rect_center_lat:.6f}, {rect_center_lon:.6f})") + + waypoints_per_drone, origin = planner.plan_formation_mission( + drone_gps_positions, target_gps, MissionType.GRID_SWEEP, + params={ + 'rect_corners': rect_corners, + 'line_spacing': GRID_LINE_SPACING, + 'altitude': BASE_ALTITUDE + } + ) + else: + print(f"未知測試模式: {TEST_MODE}") + return + + planned_waypoints = { + 'drone_ids': drone_ids, + 'waypoints': waypoints_per_drone + } + + # 印出規劃結果 + for i, did in enumerate(drone_ids): + wps = waypoints_per_drone[i] + print(f" {did}: {len(wps)} 個航點") + for j, wp in enumerate(wps): + print(f" WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f}m)") + + # 啟動任務 + print("\n啟動任務 ...") + manager.start_gps_polling(interval_ms=200) + + sender = MavlinkSender(SEND_CONNECTION) + executor = MissionExecutor( + sender=sender, + drone_gps=manager.drone_gps, + arrival_radius=ARRIVAL_RADIUS, + send_rate_hz=2.0 + ) + + executor.drone_waypoint_reached.connect( + lambda did, idx, total: print(f"\n >> {did} 到達 WP {idx}/{total}") + ) + executor.mission_completed.connect( + lambda: (print("\n===== 任務全部完成 ====="), app.quit()) + ) + + # 設定超時自動退出 + timeout_timer = QTimer() + timeout_timer.setSingleShot(True) + timeout_timer.timeout.connect(lambda: ( + print("\n⚠ 測試超時，強制退出"), + executor.stop(), + app.quit() + )) + timeout_timer.start(180_000) # 180 秒超時 + + executor.start(planned_waypoints) + + print("進入事件迴圈 (等待任務完成或 180 秒超時) ...\n") + app.exec() + + executor.stop() + sender.close() + print("測試結束") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/external/mavros_msgs/CHANGELOG.rst b/src/external/mavros_msgs/CHANGELOG.rst new file mode 100644 index 0000000..3cbecbe --- /dev/null +++ b/src/external/mavros_msgs/CHANGELOG.rst @@ -0,0 +1,1009 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mavros_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.9.0 (2024-10-10) +------------------ + +2.8.0 (2024-06-07) +------------------ +* regenerate all using cogall.sh +* Merge branch 'master' into ros2 + * master: + 1.19.0 + update changelog + gps_global_origin: remove LLA to ECEF conversion +* 1.19.0 +* update changelog +* removed prefix in enums in messages and changed to use existing functions for string and quaternion convert +* Final touches + Added functionality that was overlooked for camera tracking if supported, added copyright info, added custom exception thrown when mode enumerator is not understood +* Added gimbal_control plugin + Added all functionality to support a plugin to enable compatibility with MAVLink Gimbal Protocol v2 +* Contributors: Frederik Mazur Andersen, Mark-Beaty, Vladimir Ermakov + +1.19.0 (2024-06-06) +------------------- + +2.7.0 (2024-03-03) +------------------ +* re-generate with cogall.sh +* Merge branch 'master' into ros2 + * master: + 1.18.0 + update changelog + sys_status.cpp: improve timeout code + sys_status.cpp: Add a SYS_STATUS message publisher + [camera plugin] Fix image_index and capture_result not properly filled + Fix missing semi-colon + GPS_STATUS Plugin: Fill in available messages for ROS1 legacy +* 1.18.0 +* update changelog +* sys_status.cpp: Add a SYS_STATUS message publisher +* cog checksum +* remove event_time_boot_ms, fill stamp instead +* handle events +* Fix errata in GPSRAW.msg +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Seunghwan Jo, Vladimir Ermakov, victor + +1.18.0 (2024-03-03) +------------------- +* sys_status.cpp: Add a SYS_STATUS message publisher +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas + +2.6.0 (2023-09-09) +------------------ +* msgs: move generator code +* cog: regenerate all +* Merge branch 'master' into ros2 + * master: + 1.17.0 + update changelog + cog: regenerate all + Bugfix/update map origin with home position (`#1892 `_) + mavros: Remove extra ';' + mavros_extras: Fix some init order warnings + Suppress warnings from included headers + 1.16.0 + update changelog + made it such that the gp_origin topic published latched. + use hpp instead of deprecated .h pluginlib headers +* 1.17.0 +* update changelog +* cog: regenerate all +* local takeoff and land topics (`#1890 `_) + * local takeoff and land topics + * vector3 position type, rename to TOLLocal + * remove auto include line +* Merge pull request `#1871 `_ from Vladislavert/feature/optical_flow_msg + Addition of New OpticalFlow.msg +* Added geometry_msgs/Vector3 to OpticalFlow.msg +* Added vectors to the message OpticalFlow.msg +* Added message optical flow +* 1.16.0 +* update changelog +* Contributors: Ido Guzi, Vladimir Ermakov, Vladislavert + +2.5.0 (2023-05-05) +------------------ + +2.4.0 (2022-12-30) +------------------ +* msgs: re-generate +* Merge branch 'master' into ros2 + * master: + 1.15.0 + update changelog + ci: update actions + Implement debug float array handler + mavros_extras: Fix a sequence point warning + mavros_extras: Fix a comparison that shouldn't be bitwise + mavros: Fix some warnings + mavros_extras: Fix buggy check for lat/lon ignored + libmavconn: fix MAVLink v1.0 output selection +* 1.15.0 +* update changelog +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Contributors: Sverre Velten Rothmund, Vladimir Ermakov + +2.3.0 (2022-09-24) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.14.0 + update changelog + scripts: waypoint and param files are text, not binary + libmavconn: fix MAVLink v1.0 output selection + plugins: add guided_target to accept offboard position targets + add cmake module path for geographiclib on debian based systems + use already installed FindGeographicLib.cmake +* 1.14.0 +* update changelog +* Contributors: Vladimir Ermakov + +2.2.0 (2022-06-27) +------------------ +* Merge branch 'master' into ros2 + * master: + mount_control.cpp: detect MOUNT_ORIENTATION stale messages + ESCTelemetryItem.msg: correct RPM units + apm_config.yaml: add mount configuration + sys_status.cpp fix free memory for values > 64KiB + uncrustify cellular_status.cpp + Add CellularStatus plugin and message + *_config.yaml: document usage of multiple batteries diagnostics + sys_status.cpp: fix compilation + sys_status.cpp: support diagnostics on up-to 10 batteries + sys_status.cpp: do not use harcoded constants + sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and publish on the diagnostics + sys_status.cpp: fix enabling of mem_diag and hwst_diag + sys_status.cpp: Do not use battery1 voltage as voltage for all other batteries (bugfix). + sys_status.cpp: ignore sys_status mavlink messages from gimbals + mount_control.cpp: use mount_nh for params to keep similarities with other plugins set diag settings before add() + sys_status.cpp: remove deprecated BATTERY2 mavlink message support + Mount control plugin: add configurable diagnostics + Bugfix: increment_f had no value asigned when input LaserScan was bigger than obstacle.distances.size() + Bugfix: wrong interpolation when the reduction ratio (scale_factor) is not integer. + Disable startup_px4_usb_quirk in px4_config.yaml +* msgs: support humble + +2.1.1 (2022-03-02) +------------------ + +2.1.0 (2022-02-02) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.13.0 + update changelog + py-lib: fix compatibility with py3 for Noetic + re-generate all coglets + test: add checks for ROTATION_CUSTOM + lib: Fix rotation search for CUSTOM + Removed CamelCase for class members. Publish to "report" + More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages + Fixed callback name to match `handle\_{MESSAGE_NAME.lower()}` convention + Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html + Fixed topic names to match more closely what other plugins use. Fixed a typo. + Add plugin for reporting terrain height estimate from FCU + 1.12.2 + update changelog + Set time/publish_sim_time to false by default + plugin: setpoint_raw: move getParam to initializer + extras: trajectory: backport `#1667 `_ +* 1.13.0 +* update changelog +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* re-generate all coglets +* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames + Add extra MAV_FRAMES to waypoint message +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html +* Add plugin for reporting terrain height estimate from FCU +* 1.12.2 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.12.1 + update changelog + mavconn: fix connection issue introduced by `#1658 `_ + mavros_extras: Fix some warnings + mavros: Fix some warnings +* 1.12.1 +* update changelog +* Contributors: Vladimir Ermakov, matt + +2.0.5 (2021-11-28) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.12.0 + update changelog + Fix multiple bugs + lib: fix mission frame debug print + extras: distance_sensor: revert back to zero quaternion +* 1.12.0 +* update changelog +* extras: fix some more lint warns +* msgs: update conversion header +* Merge branch 'master' into ros2 + * master: + 1.11.1 + update changelog + lib: fix build +* 1.11.1 +* update changelog +* Merge branch 'master' into ros2 + * master: + 1.11.0 + update changelog + lib: fix ftf warnings + msgs: use pragmas to ignore unaligned pointer warnings + extras: landing_target: fix misprint + msgs: fix convert const + plugin: setpoint_raw: fix misprint + msgs: try to hide 'unaligned pointer' warning + plugin: sys: fix compillation error + plugin: initialize quaternions with identity + plugin: sys: Use wall timers for connection management + Use meters for relative altitude + distance_sensor: Initialize sensor orientation quaternion to zero + Address review comments + Add camera plugin for interfacing with mavlink camera protocol +* 1.11.0 +* update changelog +* msgs: use pragmas to ignore unaligned pointer warnings +* msgs: fix convert const +* msgs: try to hide 'unaligned pointer' warning +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* Merge branch 'master' into ros2 + * master: + msgs: add yaw field to GPS_INPUT +* msgs: add yaw field to GPS_INPUT +* Contributors: Jaeyoung-Lim, Vladimir Ermakov + +2.0.4 (2021-11-04) +------------------ +* Merge branch 'master' into ros2 + * master: + 1.10.0 + prepare release +* 1.10.0 +* prepare release +* Merge branch 'master' into ros2 + * master: + msgs: update gpsraw to have yaw field +* msgs: update gpsraw to have yaw field +* Merge branch 'master' into ros2 + * master: (25 commits) + Remove reference + Catch std::length_error in send_message + Show ENOTCONN error instead of crash + Tunnel: Check for invalid payload length + Tunnel.msg: Generate enum with cog + mavros_extras: Create tunnel plugin + mavros_msgs: Add Tunnel message + MountControl.msg: fix copy-paste + sys_time.cpp: typo + sys_time: publish /clock for simulation times + 1.9.0 + update changelog + Spelling corrections + Changed OverrideRCIn to 18 channels + This adds functionality to erase all logs on the SD card via mavlink + publish BATTERY2 message as /mavros/battery2 topic + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 + Added NAV_CONTROLLER_OUTPUT Plugin + Added GPS_INPUT plugin + Update esc_status plugin with datatype change on MAVLink. + ... +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel.msg: Generate enum with cog +* mavros_msgs: Add Tunnel message +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* MountControl.msg: fix copy-paste +* 1.9.0 +* update changelog +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Added GPS_INPUT plugin +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* msgs: re-generate file lists +* Merge branch 'master' into ros2 + * master: + extras: esc_telemetry: fix build + extras: fix esc_telemetry centi-volt/amp conversion + extras: uncrustify all plugins + plugins: reformat xml + extras: reformat plugins xml + extras: fix apm esc_telemetry + msgs: fix types for apm's esc telemetry + actually allocate memory for the telemetry information + fixed some compile errors + added esc_telemetry plugin + Reset calibration flag when re-calibrating. Prevent wrong data output. + Exclude changes to launch files. + Delete debug files. + Apply uncrustify changes. + Set progress array to global to prevent erasing data. + Move Compass calibration report to extras. Rewrite code based on instructions. + Remove extra message from CMakeLists. + Add message and service definition. + Add compass calibration feedback status. Add service to call the 'Next' button in calibrations. +* msgs: fix types for apm's esc telemetry +* actually allocate memory for the telemetry information +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Remove extra message from CMakeLists. +* Add message and service definition. +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Morten Fyhn Amundsen, Ricardo Marques, Russell, Vladimir Ermakov + +2.0.3 (2021-06-20) +------------------ + +2.0.2 (2021-06-20) +------------------ + +2.0.1 (2021-06-06) +------------------ +* Add rcl_interfaces dependency +* Merge branch 'master' into ros2 + * master: + readme: update + 1.8.0 + update changelog + Create semgrep-analysis.yml + Create codeql-analysis.yml +* 1.8.0 +* update changelog +* Contributors: Rob Clarke, Vladimir Ermakov + +2.0.0 (2021-05-28) +------------------ +* msgs: update command codes +* msgs: update param services +* plugins: setpoint_velocity: port to ros2 +* Merge branch 'master' into ros2 + * master: + 1.7.1 + update changelog + re-generate all pymavlink enums + 1.7.0 + update changelog +* mavros: generate plugin list +* Merge branch 'master' into ros2 + * master: + msgs: re-generate the code + lib: re-generate the code + plugins: mission: re-generate the code + MissionBase: correction to file information + MissionBase: add copyright from origional waypoint.cpp + uncrustify + whitespace + add rallypoint and geofence plugins to mavros plugins xml + add rallypoint and geofence plugins to CMakeList + Geofence: add geofence plugin + Rallypoint: add rallypoint plugin + Waypoint: inherit MissionBase class for mission protocol + MissionBase: breakout mission protocol from waypoint.cpp + README: Update PX4 Autopilot references + Fix https://github.com/mavlink/mavros/issues/849 +* router: catch DeviceError +* router: weak_ptr segfaults, replace with shared_ptr +* router: implement params handler +* mavros: router decl done +* lib: port enum_to_string +* lib: update sensor_orientation +* msgs: add linter +* libmavconn: start porintg, will use plain asio, without boost +* msgs: remove redundant dependency which result in colcon warning +* msgs: cogify file lists +* Merge pull request `#1186 `_ from PickNikRobotics/ros2 + mavros_msgs Ros2 +* Merge branch 'ros2' into ros2 +* msgs: start porting to ROS2 +* fixing cmakelists +* updating msg and srv list +* reenable VfrHud once renamed to match ROS2 conventions + add ros1_bridge mapping rule for renamed VfrHud message +* make mavro_msgs compile in ROS 2 +* Contributors: Mikael Arguedas, Mike Lautman, Vladimir Ermakov + +1.17.0 (2023-09-09) +------------------- +* cog: regenerate all +* Contributors: Vladimir Ermakov + +1.16.0 (2023-05-05) +------------------- + +1.15.0 (2022-12-30) +------------------- +* Merge pull request `#1811 `_ from scoutdi/debug-float-array + Implement debug float array handler +* Implement debug float array handler + Co-authored-by: Morten Fyhn Amundsen +* Contributors: Sverre Velten Rothmund, Vladimir Ermakov + +1.14.0 (2022-09-24) +------------------- +* Merge pull request `#1742 `_ from amilcarlucas/correct_rpm_units + ESCTelemetryItem.msg: correct RPM units +* ESCTelemetryItem.msg: correct RPM units +* Merge pull request `#1727 `_ from BV-OpenSource/pr-cellular-status + Pr cellular status +* Add CellularStatus plugin and message +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Vladimir Ermakov + +1.13.0 (2022-01-13) +------------------- +* Merge pull request `#1690 `_ from mavlink/fix-enum_sensor_orientation + Fix enum sensor_orientation +* re-generate all coglets +* Merge pull request `#1680 `_ from AndersonRayner/new_mav_frames + Add extra MAV_FRAMES to waypoint message +* Merge pull request `#1677 `_ from AndersonRayner/add_terrain + Add plugin for reporting terrain height estimate from the FCU +* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages +* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html +* Add plugin for reporting terrain height estimate from FCU +* Contributors: Vladimir Ermakov, matt + +1.12.2 (2021-12-12) +------------------- + +1.12.1 (2021-11-29) +------------------- + +1.12.0 (2021-11-27) +------------------- + +1.11.1 (2021-11-24) +------------------- + +1.11.0 (2021-11-24) +------------------- +* msgs: use pragmas to ignore unaligned pointer warnings +* msgs: fix convert const +* msgs: try to hide 'unaligned pointer' warning +* Merge pull request `#1651 `_ from Jaeyoung-Lim/pr-image-capture-plugin + Add camera plugin for interfacing with mavlink camera protocol +* Address review comments +* Add camera plugin for interfacing with mavlink camera protocol + Add camera image captured message for handling camera trigger information +* msgs: add yaw field to GPS_INPUT +* Contributors: Jaeyoung-Lim, Vladimir Ermakov + +1.10.0 (2021-11-04) +------------------- +* msgs: update gpsraw to have yaw field +* Merge pull request `#1625 `_ from scoutdi/tunnel-plugin + Plugin for TUNNEL messages +* Tunnel.msg: Generate enum with cog +* mavros_msgs: Add Tunnel message +* Merge pull request `#1623 `_ from amilcarlucas/pr/more-typo-fixes + More typo fixes +* MountControl.msg: fix copy-paste +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Morten Fyhn Amundsen, Vladimir Ermakov + +1.9.0 (2021-09-09) +------------------ +* Merge pull request `#1616 `_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions + Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.… +* Changed OverrideRCIn to 18 channels +* Merge pull request `#1617 `_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin + Added NAV_CONTROLLER_OUTPUT Plugin +* Merge pull request `#1618 `_ from amilcarlucas/pr/GPS_INPUT-plugin + Added GPS_INPUT plugin +* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0 +* Added NAV_CONTROLLER_OUTPUT Plugin +* Added GPS_INPUT plugin +* Merge branch 'master' into master +* Update esc_status plugin with datatype change on MAVLink. + ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side. +* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message. +* msgs: fix types for apm's esc telemetry +* actually allocate memory for the telemetry information +* added esc_telemetry plugin +* Add Mount angles message for communications with ardupilotmega. +* Remove extra message from CMakeLists. +* Add message and service definition. +* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Ricardo Marques, Russell, Vladimir Ermakov + +1.8.0 (2021-05-05) +------------------ + +1.7.1 (2021-04-05) +------------------ +* re-generate all pymavlink enums +* Contributors: Vladimir Ermakov + +1.7.0 (2021-04-05) +------------------ +* msgs: re-generate the code +* Contributors: Vladimir Ermakov + +1.6.0 (2021-02-15) +------------------ + +1.5.2 (2021-02-02) +------------------ + +1.5.1 (2021-01-04) +------------------ + +1.5.0 (2020-11-11) +------------------ +* mavros_msgs/VehicleInfo: Add flight_custom_version field + Mirroring the field in the corresponding MAVLink message. +* mavros_msgs/State: Fix PX4 flight mode constants + Turns out ROS message string literals don't need quotes, + so adding quotes creates strings including the quotes. +* mavros_msgs/State: Add flight mode constants +* mavros_msgs: Don't move temporary objects +* Contributors: Morten Fyhn Amundsen + +1.4.0 (2020-09-11) +------------------ +* play_tune: Assign tune format directly +* play_tune: Write new plugin +* Contributors: Morten Fyhn Amundsen + +1.3.0 (2020-08-08) +------------------ +* Add esc_status plugin. +* Add gps_status plugin to publish GPS_RAW and GPS_RTK messages from FCU. + The timestamps for the gps_status topics take into account the mavlink time and uses the convienence function +* adding support for publishing rtkbaseline msgs over ROS +* Contributors: CSCE439, Dr.-Ing. Amilcar do Carmo Lucas, Ricardo Marques + +1.2.0 (2020-05-22) +------------------ +* add yaw to CMD_DO_SET_HOME +* Contributors: David Jablonski + +1.1.0 (2020-04-04) +------------------ + +1.0.0 (2020-01-01) +------------------ + +0.33.4 (2019-12-12) +------------------- +* Splitted the message fields. +* Updated esimator status msg according to the new cog based definition of estimator status. +* Added comments to msg. +* Added new line char at end of message. +* Added a publisher for estimator status message received from mavlink in sys_status. +* Contributors: saifullah3396 + +0.33.3 (2019-11-13) +------------------- + +0.33.2 (2019-11-13) +------------------- + +0.33.1 (2019-11-11) +------------------- +* resolved merge conflict +* Contributors: David Jablonski + +0.33.0 (2019-10-10) +------------------- +* Add vtol transition service +* Apply comments +* Add mount configure service message +* cog: Update all generated code +* added manual flag to mavros/state +* use header.stamp to fill mavlink msg field time_usec +* use cog for copy +* adapt message and plugin after mavlink message merge +* rename message and adjust fields +* add component id to mavros message to distinguish ROS msgs from different systems +* component_status message and plugin draft +* Contributors: David Jablonski, Jaeyoung-Lim, Vladimir Ermakov, baumanta + +0.32.2 (2019-09-09) +------------------- + +0.32.1 (2019-08-08) +------------------- + +0.32.0 (2019-07-06) +------------------- +* add mav_cmd associated with each point in trajectory plugin +* Use MountControl Msg +* Define new MountControl.msg +* Contributors: Jaeyoung-Lim, Martina Rivizzigno + +0.31.0 (2019-06-07) +------------------- +* mavros_msgs: LandingTarget: update msg description link +* extras: landing target: improve usability and flexibility +* Contributors: TSC21 + +0.30.0 (2019-05-20) +------------------- + +0.29.2 (2019-03-06) +------------------- + +0.29.1 (2019-03-03) +------------------- +* All: catkin lint files +* mavros_msgs: Fix line endings for OpticalFlowRad message +* Contributors: Pierre Kancir, sfalexrog + +0.29.0 (2019-02-02) +------------------- +* Fix broken documentation URLs +* Merge branch 'master' into param-timeout +* mavros_extras: Wheel odometry plugin updated according to the final mavlink WHEEL_DISTANCE message. +* mavros_msgs: Float32ArrayStamped replaced by WheelOdomStamped. +* mavros_msgs: Float32ArrayStamped message added. + For streaming timestamped data from FCU sensors (RPM, WHEEL_DISTANCE, etc.) +* msgs: Fix message id type, mavlink v2 uses 24 bit msg ids +* mavros_msgs: add MessageInterval.srv to CMakeLists +* sys_status: add set_message_interval service +* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Pavlo Kolomiiets, Randy Mackay, Vladimir Ermakov + +0.28.0 (2019-01-03) +------------------- +* plugin:param: publish new param value +* Merge pull request `#1148 `_ from Kiwa21/pr-param-value + param plugin : add msg and publisher to catch latest param value +* msgs: update Header +* sys_state: Small cleanup of `#1150 `_ +* VehicleInfo : add srv into sys_status plugin to request basic info from vehicle +* mavros_msgs/msg/LogData.msg: Define "offset" field to be of type uint32 +* param plugin : add msg and publisher to catch latest param value +* style clean up +* Use component_id to determine message sender +* change message name from COMPANION_STATUS to COMPANION_PROCESS_STATUS +* change message to include pid +* Change from specific avoidance status message to a more generic companion status message +* Add message for avoidance status +* Contributors: Gregoire Linard, Vladimir Ermakov, baumanta, mlvov + +0.27.0 (2018-11-12) +------------------- +* Add service to send mavlink TRIGG_INTERVAL commands + Adapt trigger_control service to current mavlink cmd spec. Add a new service to change trigger interval and integration time +* Contributors: Moritz Zimmermann + +0.26.3 (2018-08-21) +------------------- +* fixup! 5a4344a2dcedc157f93b620cebd2e0b273ec24be +* mavros_msgs: Add msg and srv files related to log transfer +* Contributors: mlvov + +0.26.2 (2018-08-08) +------------------- +* Updating the gps_rtk plugin to fit mavros guidelines: + - Updating max_frag_len to allow changes in size in MAVLink seamlessly + - Using std::copy instead of memset + - Zero fill with std::fill + - Preapply the sequence flags + - Use of std iterators + - Add the maximal data size in the mavros_msgs +* Renaming the GPS RTK module, Adding fragmentation, Changing the RTCM message +* RTK Plugin; to forward RTCM messages + Signed-off-by: Alexis Paques +* Contributors: Alexis Paques + +0.26.1 (2018-07-19) +------------------- + +0.26.0 (2018-06-06) +------------------- +* mavros_msgs : add timesync status message +* Contributors: Mohammed Kabir + +0.25.1 (2018-05-14) +------------------- + +0.25.0 (2018-05-11) +------------------- +* trajectory: add time_horizon field +* change message name from ObstacleAvoidance to Trajectory since it is + general enough to support any type of trajectory +* CMakeLists: add ObstacleAvoidance message +* add ObstacleAvoidance message +* msgs: Update message doc link +* CommandCode: update list of available commands on MAV_CMD enum (`#995 `_) +* Contributors: Martina, Nuno Marques, Vladimir Ermakov + +0.24.0 (2018-04-05) +------------------- +* Add ability to send STATUSTEXT messages +* Contributors: Anass Al + +0.23.3 (2018-03-09) +------------------- + +0.23.2 (2018-03-07) +------------------- + +0.23.1 (2018-02-27) +------------------- + +0.23.0 (2018-02-03) +------------------- + +0.22.0 (2017-12-11) +------------------- +* SetMavFrame.srv: add FRAME\_ prefix +* Add cog for SetMavFrame.srv +* Setpoints: add service to specify frame +* Contributors: Pierre Kancir, khancyr + +0.21.5 (2017-11-16) +------------------- + +0.21.4 (2017-11-01) +------------------- + +0.21.3 (2017-10-28) +------------------- +* plugin waypoints: Use stamped message +* add debug plugin +* Contributors: TSC21, Vladimir Ermakov + +0.21.2 (2017-09-25) +------------------- + +0.21.1 (2017-09-22) +------------------- + +0.21.0 (2017-09-14) +------------------- +* plugin waypoint: Rename current seq in wp list message +* waypoint: Publish current waypoint seq +* waypoint partial: code style cleanup +* waypoint partial: extend existing service +* Partial waypoint: added wp_transfered to push partial service response +* Partial waypoint: added partial updating to mavwp +* Contributors: James Mare, James Stewart, Vladimir Ermakov + +0.20.1 (2017-08-28) +------------------- + +0.20.0 (2017-08-23) +------------------- +* HIL Plugin + * add HilSensor.msg, HilStateQuaternion.msg, and add them in CMakeLists.txt + * Add hil_sensor.cpp plugin to send HIL_SENSOR mavlink message to FCU. + * fix HilSensor.msg. Make it more compact. + * Fix HilStateQuaternion.msg. Make it more compact. + * Add hil_state_quaternion plugin + * fix files: some variable names were wrong+some syntax problems + * fix syntax error in plugin .cpp files, make msg files match corresponding mavlink definitions + * fix plugin source files + * fix syntax + * fix function name. It was wrong. + * add HIL_GPS plugin + * add HilGPS.msg to CMakeList + * fix missing semicolon + * fix call of class name + * Add ACTUATOR_CONTROL_TARGET MAVLink message + * fix code + * increase number of fake satellites + * control sensor and control rates + * change control rate + * change control rate + * fix fake gps rate + * fix + * fix plugin_list + * fix + * remove unnecessary hil_sensor_mixin + * update HilSensor.msg and usage + * update HilStateQuaterion.msg and usage + * redo some changes; update HilGPS.msg and usage + * update hil_controls msg - use array of floats for aux channels + * merge actuator_control with actuator_control_target + * remove hil_sensor_mixin.h + * update actuator_control logic + * merge all plugins into a single one + * delete the remaining plugin files + * update description + * redo some changes; reduce LOC + * fix type cast on gps coord + * add HIL_OPTICAL_FLOW send based on OpticalFlowRad sub + * update authors list + * update subscribers names + * refactor gps coord convention + * add HIL_RC_INPUTS_RAW sender; cog protec msg structure and content + * apply correct rc_in translation; redo cog + * apply proper rotations and frame transforms + * remote throttle + * fix typo and msg api + * small changes + * refactor rcin_raw_cb + * new refactor to rcin_raw_cb arrays + * update velocity to meters + * readjust all the units so to match mavlink msg def + * update cog + * correct cog conversion + * refefine msg definitions to remove overhead + * hil: apply frame transform to body frame +* msgs fix `#625 `_: Rename SetMode.Response.success to mode_sent +* [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (`#700 `_) + * plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together + * plugins: typo correction: replace throttle with thrust + * plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg + * plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust) + * setpoint_attitude: update the logic of thrust normalization verification + * setpoint_attitude: implement sync between tf listener and thrust subscriber + * TF sync listener: generalize topic type that can be syncronized with TF2 + * TF2ListenerMixin: keep class template, use template for tf sync method only + * TF2ListenerMixin: fix and improve sync tf2_start method + * general update to yaml config files and parameters + * setpoint_attitude: add note on Thrust sub name + * setpoint_attitude: TF sync: pass subscriber pointer instead of binding it +* Use GeographicLib tools to guarantee ROS msg def and enhance features (`#693 `_) + * first commit + * Check for GeographicLib first without having to install it from the beginning each compile time + * add necessary cmake files + * remove gps_conversions.h and use GeographicLib to obtain the UTM coordinates + * move conversion functions to utils.h + * geographic conversions: update CMakeLists and package.xml + * geographic conversions: force download of the datasets + * geographic conversions: remove unneeded cmake module + * dependencies: use SHARED libs of geographiclib + * dependencies: correct FindGeographicLib.cmake so it can work for common Debian platforms + * CMakeList: do not be so restrict about GeographicLib dependency + * global position: odometry-use ECEF instead of UTM; update other fields + * global position: make travis happy + * global position: fix ident + * global_position: apply correct frames and frame transforms given each coordinate frame + * global_position: convert rcvd global origin to ECEF + * global_position: be more explicit about the ecef-enu transform + * global position: use home position as origin of map frame + * global position: minor refactoring + * global position: shield code with exception catch + * fix identation + * move dataset install to script; update README with new functionalities + * update README with warning + * global_position: fix identation + * update HomePosition to be consistent with the conversions in global_position to ensure the correct transformation of height + * home|global_position: fix compile errors, logic and dependencies + * home position: add height conversion + * travis: update to get datasets + * install geo dataset: update to verify alternative dataset folders + * travis: remove dataset install to allow clean build + * hp and gp: initialize geoid dataset once and make it thread safe + * README: update description relative to GeographicLib; fix typos + * global position: improve doxygen references + * README: update with some tips on rosdep install +* update ExtendedState with new MAV_LANDED_STATE enum +* Contributors: Nicklas Stockton, Nuno Marques, Vladimir Ermakov + +0.19.0 (2017-05-05) +------------------- +* msgs: Add cog script to finish ADSBVehicle.msg +* extras: Add ADSB plugin +* plugin `#695 `_: Fix plugin +* plugin: Add home_position +* Contributors: Nuno Marques, Vladimir Ermakov + +0.18.7 (2017-02-24) +------------------- +* trigger interface : rename to cycle_time to be consistent with PX4 +* Contributors: Kabir Mohammed + +0.18.6 (2017-02-07) +------------------- +* Plugins: system_status change status field to system_status + Add comment to State.msg for system_status enum +* Plugins: add system_status to state message +* Contributors: Pierre Kancir + +0.18.5 (2016-12-12) +------------------- + +0.18.4 (2016-11-11) +------------------- +* msgs: Fix `#609 `_ +* add hil_actuator_controls mavlink message +* Contributors: Beat Kung, Vladimir Ermakov + +0.18.3 (2016-07-07) +------------------- + +0.18.2 (2016-06-30) +------------------- + +0.18.1 (2016-06-24) +------------------- + +0.18.0 (2016-06-23) +------------------- +* Adding anchor to the HIL_CONTROLS message reference link +* Utilizing synchronise_stamp and adding reference to MAVLINK msg documentation +* Added a plugin that publishes HIL_CONTROLS as ROS messages +* node: Rename plugib base class - API incompatible to old class +* msgs `#543 `_: Update for MAVLink 2.0 +* Contributors: Pavel, Vladimir Ermakov + +0.17.3 (2016-05-20) +------------------- + +0.17.2 (2016-04-29) +------------------- + +0.17.1 (2016-03-28) +------------------- + +0.17.0 (2016-02-09) +------------------- +* rebased with master +* Contributors: francois + +0.16.6 (2016-02-04) +------------------- + +0.16.5 (2016-01-11) +------------------- + +0.16.4 (2015-12-14) +------------------- +* Update mavlink message documentation links +* remove "altitude\_" prefix from members +* implemented altitude plugin +* Contributors: Andreas Antener, Vladimir Ermakov + +0.16.3 (2015-11-19) +------------------- + +0.16.2 (2015-11-17) +------------------- + +0.16.1 (2015-11-13) +------------------- + +0.16.0 (2015-11-09) +------------------- +* msgs `#418 `_: add message for attitude setpoints +* plugin: waypoint fix `#414 `_: remove GOTO service. + It is replaced with more standard global setpoint messages. +* msgs `#415 `_: Add message for raw global setpoint +* msgs `#402 `_: PositionTarget message type +* setting constant values and reference docs +* pass new extended state to ros +* msgs `#371 `_: add missing message +* msgs `#371 `_: add HomePosition message +* Contributors: Andreas Antener, Vladimir Ermakov + +0.15.0 (2015-09-17) +------------------- +* msgs `#286 `_: fix bug with packet header. +* msgs `#286 `_: Add valid flag and checksum to Mavlink.msg +* plugin: manual_control: Use shared pointer message + Fix alphabetic order of msgs. +* removed old commend in .msg file +* Add MANUAL_CONTROL handling with new plugin +* Contributors: Vladimir Ermakov, v01d + +0.14.2 (2015-08-20) +------------------- + +0.14.1 (2015-08-19) +------------------- + +0.14.0 (2015-08-17) +------------------- +* msgs: Add mixer group constants ActuatorControl +* msgs: Add notes to message headers. +* msgs: sort msgs in alphabetical order +* msgs: use std::move for mavlink->ros convert +* msgs: add note about convert function +* msgs: change description, make catkin lint happy +* msgs: move convert functions to msgs package. +* msgs: fix message generator and runtime depent tags +* msgs: remove never used Mavlink.fromlcm field. +* msgs: add package name for all non basic types +* msgs: fix msgs build +* msgs `#354 `_: move all messages to mavros_msgs package. +* Contributors: Vladimir Ermakov diff --git a/src/external/mavros_msgs/CMakeLists.txt b/src/external/mavros_msgs/CMakeLists.txt new file mode 100644 index 0000000..a3220b0 --- /dev/null +++ b/src/external/mavros_msgs/CMakeLists.txt @@ -0,0 +1,208 @@ +cmake_minimum_required(VERSION 3.5) +project(mavros_msgs) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rcl_interfaces REQUIRED) +find_package(geographic_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +# include_directories(include) + +# [[[cog: +# import mavros_cog +# ]]] +# [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +set(msg_files + # [[[cog: + # mavros_cog.outl_glob_files('msg', '*.msg') + # ]]] + msg/ADSBVehicle.msg + msg/ActuatorControl.msg + msg/Altitude.msg + msg/AttitudeTarget.msg + msg/CamIMUStamp.msg + msg/CameraImageCaptured.msg + msg/CellularStatus.msg + msg/CommandCode.msg + msg/CompanionProcessStatus.msg + msg/DebugValue.msg + msg/ESCInfo.msg + msg/ESCInfoItem.msg + msg/ESCStatus.msg + msg/ESCStatusItem.msg + msg/ESCTelemetry.msg + msg/ESCTelemetryItem.msg + msg/EstimatorStatus.msg + msg/ExtendedState.msg + msg/FileEntry.msg + msg/GPSINPUT.msg + msg/GPSRAW.msg + msg/GPSRTK.msg + msg/GimbalDeviceAttitudeStatus.msg + msg/GimbalDeviceInformation.msg + msg/GimbalDeviceSetAttitude.msg + msg/GimbalManagerInformation.msg + msg/GimbalManagerSetAttitude.msg + msg/GimbalManagerSetPitchyaw.msg + msg/GimbalManagerStatus.msg + msg/GlobalPositionTarget.msg + msg/HilActuatorControls.msg + msg/HilControls.msg + msg/HilGPS.msg + msg/HilSensor.msg + msg/HilStateQuaternion.msg + msg/HomePosition.msg + msg/LandingTarget.msg + msg/LogData.msg + msg/LogEntry.msg + msg/MagnetometerReporter.msg + msg/ManualControl.msg + msg/Mavlink.msg + msg/MountControl.msg + msg/NavControllerOutput.msg + msg/OnboardComputerStatus.msg + msg/OpticalFlow.msg + msg/OpticalFlowRad.msg + msg/OverrideRCIn.msg + msg/Param.msg + msg/ParamEvent.msg + msg/ParamValue.msg + msg/PlayTuneV2.msg + msg/PositionTarget.msg + msg/RCIn.msg + msg/RCOut.msg + msg/RTCM.msg + msg/RTKBaseline.msg + msg/RadioStatus.msg + msg/State.msg + msg/StatusEvent.msg + msg/StatusText.msg + msg/SysStatus.msg + msg/TerrainReport.msg + msg/Thrust.msg + msg/TimesyncStatus.msg + msg/Trajectory.msg + msg/Tunnel.msg + msg/VehicleInfo.msg + msg/VfrHud.msg + msg/Vibration.msg + msg/Waypoint.msg + msg/WaypointList.msg + msg/WaypointReached.msg + msg/WheelOdomStamped.msg + # [[[end]]] (checksum: a8e24eb0a6da5cea6cc049fdc6b2612e) +) + +set(srv_files + # [[[cog: + # mavros_cog.outl_glob_files('srv', '*.srv') + # ]]] + srv/CommandAck.srv + srv/CommandBool.srv + srv/CommandHome.srv + srv/CommandInt.srv + srv/CommandLong.srv + srv/CommandTOL.srv + srv/CommandTOLLocal.srv + srv/CommandTriggerControl.srv + srv/CommandTriggerInterval.srv + srv/CommandVtolTransition.srv + srv/EndpointAdd.srv + srv/EndpointDel.srv + srv/FileChecksum.srv + srv/FileClose.srv + srv/FileList.srv + srv/FileMakeDir.srv + srv/FileOpen.srv + srv/FileRead.srv + srv/FileRemove.srv + srv/FileRemoveDir.srv + srv/FileRename.srv + srv/FileTruncate.srv + srv/FileWrite.srv + srv/GimbalGetInformation.srv + srv/GimbalManagerCameraTrack.srv + srv/GimbalManagerConfigure.srv + srv/GimbalManagerPitchyaw.srv + srv/GimbalManagerSetRoi.srv + srv/LogRequestData.srv + srv/LogRequestEnd.srv + srv/LogRequestList.srv + srv/MessageInterval.srv + srv/MountConfigure.srv + srv/ParamGet.srv + srv/ParamPull.srv + srv/ParamPush.srv + srv/ParamSet.srv + srv/ParamSetV2.srv + srv/SetMavFrame.srv + srv/SetMode.srv + srv/StreamRate.srv + srv/VehicleInfoGet.srv + srv/WaypointClear.srv + srv/WaypointPull.srv + srv/WaypointPush.srv + srv/WaypointSetCurrent.srv + # [[[end]]] (checksum: cd7701b28a3176d96ef65cb1f2157917) +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES + builtin_interfaces + rcl_interfaces + geographic_msgs + geometry_msgs + sensor_msgs + std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +install( + FILES mavros_msgs_mapping_rule.yaml + DESTINATION share/${PROJECT_NAME} +) + +if(rcl_interfaces_VERSION VERSION_LESS "1.2.0") + install( + DIRECTORY include/ + DESTINATION include + FILES_MATCHING PATTERN "*.hpp" + ) +else() + # NOTE(vooon): Humble + install( + DIRECTORY include/ + DESTINATION include/mavros_msgs + FILES_MATCHING PATTERN "*.hpp" + ) +endif() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # NOTE(vooon): Does not support our custom triple-license, tiered to make it to work. + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_copyright) + + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() +# vim: ts=2 sw=2 et: diff --git a/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp new file mode 100644 index 0000000..6241e8a --- /dev/null +++ b/src/external/mavros_msgs/include/mavros_msgs/mavlink_convert.hpp @@ -0,0 +1,145 @@ +// +// Copyright 2015,2016,2021 Vladimir Ermakov. +// +// This file is part of the mavros package and subject to the license terms +// in the top-level LICENSE file of the mavros repository. +// https://github.com/mavlink/mavros/tree/master/LICENSE.md +// +/** + * @brief Mavlink convert utils + * @file + * @author Vladimir Ermakov + */ + +#pragma once +#ifndef MAVROS_MSGS__MAVLINK_CONVERT_HPP_ +#define MAVROS_MSGS__MAVLINK_CONVERT_HPP_ + +#include +#include + +#include + +namespace mavros_msgs +{ +namespace mavlink +{ + +using ::mavlink::mavlink_message_t; +using mavros_msgs::msg::Mavlink; + +// [[[cog: +// FIELD_NAMES = [ +// "magic", +// "len", +// "incompat_flags", +// "compat_flags", +// "seq", +// "sysid", +// "compid", +// "msgid", +// "checksum", +// ] +// ]]] +// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e) + +// NOTE(vooon): Ignore impossible warning as +// memcpy() should work with unaligned pointers without any trouble. +// +// warning: taking address of packed member of ‘mavlink::__mavlink_message’ +// may result in an unaligned pointer value +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Waddress-of-packed-member" + +/** + * @brief Convert mavros_msgs/Mavlink message to mavlink_message_t + * + * @note signature vector should be empty for unsigned OR + * MAVLINK_SIGNATURE_BLOCK size for signed messages + * + * @param[in] rmsg mavros_msgs/Mavlink message + * @param[out] mmsg mavlink_message_t struct + * @return true if success + */ +inline bool convert(const Mavlink & rmsg, mavlink_message_t & mmsg) +{ + if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) { + return false; + } + + if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) { + return false; + } + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"mmsg.{f} = rmsg.{f};") + // ]]] + mmsg.magic = rmsg.magic; + mmsg.len = rmsg.len; + mmsg.incompat_flags = rmsg.incompat_flags; + mmsg.compat_flags = rmsg.compat_flags; + mmsg.seq = rmsg.seq; + mmsg.sysid = rmsg.sysid; + mmsg.compid = rmsg.compid; + mmsg.msgid = rmsg.msgid; + mmsg.checksum = rmsg.checksum; + // [[[end]]] (checksum: 0b66f0fc1cd46db0f18a2429c56a6b8c) + std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64); + std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature); + + return true; +} + +/** + * @brief Convert mavlink_message_t to mavros/Mavlink + * + * @param[in] mmsg mavlink_message_t struct + * @param[out] rmsg mavros_msgs/Mavlink message + * @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE) + * @return true, this convertion can't fail + */ +inline bool convert( + const mavlink_message_t & mmsg, Mavlink & rmsg, + uint8_t framing_status = Mavlink::FRAMING_OK) +{ + const size_t payload64_len = (mmsg.len + 7) / 8; + + rmsg.framing_status = framing_status; + + // [[[cog: + // for f in FIELD_NAMES: + // cog.outl(f"rmsg.{f} = mmsg.{f};") + // ]]] + rmsg.magic = mmsg.magic; + rmsg.len = mmsg.len; + rmsg.incompat_flags = mmsg.incompat_flags; + rmsg.compat_flags = mmsg.compat_flags; + rmsg.seq = mmsg.seq; + rmsg.sysid = mmsg.sysid; + rmsg.compid = mmsg.compid; + rmsg.msgid = mmsg.msgid; + rmsg.checksum = mmsg.checksum; + // [[[end]]] (checksum: 64ef6c1af60c622ed427e005d8ca4f2a) + rmsg.payload64.assign( + mmsg.payload64, + mmsg.payload64 + payload64_len); + + // copy signature block only if message is signed + if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED) { + rmsg.signature.assign( + mmsg.signature, + mmsg.signature + sizeof(mmsg.signature)); + } else { + rmsg.signature.clear(); + } + + return true; +} + +#pragma GCC diagnostic pop + +} // namespace mavlink +} // namespace mavros_msgs + +#endif // MAVROS_MSGS__MAVLINK_CONVERT_HPP_ diff --git a/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml new file mode 100644 index 0000000..9f69364 --- /dev/null +++ b/src/external/mavros_msgs/mavros_msgs_mapping_rule.yaml @@ -0,0 +1,8 @@ +# This file defines mappings between ROS 1 and ROS 2 interfaces. +# It is used with the ros1_bridge to allow for communcation between ROS 1 and ROS 2. + +- + ros1_package_name: 'mavros_msgs' + ros1_message_name: 'VFR_HUD' + ros2_package_name: 'mavros_msgs' + ros2_message_name: 'VfrHud' diff --git a/src/external/mavros_msgs/msg/ADSBVehicle.msg b/src/external/mavros_msgs/msg/ADSBVehicle.msg new file mode 100644 index 0000000..44b9caa --- /dev/null +++ b/src/external/mavros_msgs/msg/ADSBVehicle.msg @@ -0,0 +1,66 @@ +# The location and information of an ADSB vehicle +# +# https://mavlink.io/en/messages/common.html#ADSB_VEHICLE + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('ADSB_ALTITUDE_TYPE', 'ALT_') +# mavros_cog.idl_decl_enum('ADSB_EMITTER_TYPE', 'EMITTER_') +# mavros_cog.idl_decl_enum('ADSB_FLAGS', 'FLAG_', 16) +# ]]] +# ADSB_ALTITUDE_TYPE +uint8 ALT_PRESSURE_QNH = 0 # Altitude reported from a Baro source using QNH reference +uint8 ALT_GEOMETRIC = 1 # Altitude reported from a GNSS source +# ADSB_EMITTER_TYPE +uint8 EMITTER_NO_INFO = 0 +uint8 EMITTER_LIGHT = 1 +uint8 EMITTER_SMALL = 2 +uint8 EMITTER_LARGE = 3 +uint8 EMITTER_HIGH_VORTEX_LARGE = 4 +uint8 EMITTER_HEAVY = 5 +uint8 EMITTER_HIGHLY_MANUV = 6 +uint8 EMITTER_ROTOCRAFT = 7 +uint8 EMITTER_UNASSIGNED = 8 +uint8 EMITTER_GLIDER = 9 +uint8 EMITTER_LIGHTER_AIR = 10 +uint8 EMITTER_PARACHUTE = 11 +uint8 EMITTER_ULTRA_LIGHT = 12 +uint8 EMITTER_UNASSIGNED2 = 13 +uint8 EMITTER_UAV = 14 +uint8 EMITTER_SPACE = 15 +uint8 EMITTER_UNASSGINED3 = 16 +uint8 EMITTER_EMERGENCY_SURFACE = 17 +uint8 EMITTER_SERVICE_SURFACE = 18 +uint8 EMITTER_POINT_OBSTACLE = 19 +# ADSB_FLAGS +uint16 FLAG_VALID_COORDS = 1 +uint16 FLAG_VALID_ALTITUDE = 2 +uint16 FLAG_VALID_HEADING = 4 +uint16 FLAG_VALID_VELOCITY = 8 +uint16 FLAG_VALID_CALLSIGN = 16 +uint16 FLAG_VALID_SQUAWK = 32 +uint16 FLAG_SIMULATED = 64 +uint16 FLAG_VERTICAL_VELOCITY_VALID = 128 +uint16 FLAG_BARO_VALID = 256 +uint16 FLAG_SOURCE_UAT = 32768 +# [[[end]]] (checksum: a34f2a081739921b6e3e443ed0516d8d) + +std_msgs/Header header + +uint32 icao_address +string callsign + +float64 latitude +float64 longitude +float32 altitude # AMSL + +float32 heading # deg [0..360) +float32 hor_velocity # m/s +float32 ver_velocity # m/s + +uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum +uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum + +builtin_interfaces/Duration tslc # Duration from last communication, seconds [0..255] +uint16 flags # ADSB_FLAGS bit field +uint16 squawk # Squawk code diff --git a/src/external/mavros_msgs/msg/ActuatorControl.msg b/src/external/mavros_msgs/msg/ActuatorControl.msg new file mode 100644 index 0000000..5332a08 --- /dev/null +++ b/src/external/mavros_msgs/msg/ActuatorControl.msg @@ -0,0 +1,16 @@ +# raw servo values for direct actuator controls +# +# about groups, mixing and channels: +# https://pixhawk.org/dev/mixing + +# constant for mixer group +uint8 PX4_MIX_FLIGHT_CONTROL = 0 +uint8 PX4_MIX_FLIGHT_CONTROL_VTOL_ALT = 1 +uint8 PX4_MIX_PAYLOAD = 2 +uint8 PX4_MIX_MANUAL_PASSTHROUGH = 3 +#uint8 PX4_MIX_FC_MC_VIRT = 4 +#uint8 PX4_MIX_FC_FW_VIRT = 5 + +std_msgs/Header header +uint8 group_mix +float32[8] controls diff --git a/src/external/mavros_msgs/msg/Altitude.msg b/src/external/mavros_msgs/msg/Altitude.msg new file mode 100644 index 0000000..81ae0a9 --- /dev/null +++ b/src/external/mavros_msgs/msg/Altitude.msg @@ -0,0 +1,12 @@ +# Altitude information +# +# https://mavlink.io/en/messages/common.html#ALTITUDE + +std_msgs/Header header + +float32 monotonic +float32 amsl +float32 local +float32 relative +float32 terrain +float32 bottom_clearance diff --git a/src/external/mavros_msgs/msg/AttitudeTarget.msg b/src/external/mavros_msgs/msg/AttitudeTarget.msg new file mode 100644 index 0000000..a62712b --- /dev/null +++ b/src/external/mavros_msgs/msg/AttitudeTarget.msg @@ -0,0 +1,17 @@ +# Message for SET_ATTITUDE_TARGET +# +# Some complex system requires all feautures that mavlink +# message provide. See issue #402, #418. + +std_msgs/Header header + +uint8 type_mask +uint8 IGNORE_ROLL_RATE = 1 # body_rate.x +uint8 IGNORE_PITCH_RATE = 2 # body_rate.y +uint8 IGNORE_YAW_RATE = 4 # body_rate.z +uint8 IGNORE_THRUST = 64 +uint8 IGNORE_ATTITUDE = 128 # orientation field + +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 body_rate +float32 thrust diff --git a/src/external/mavros_msgs/msg/CamIMUStamp.msg b/src/external/mavros_msgs/msg/CamIMUStamp.msg new file mode 100644 index 0000000..52b9e8b --- /dev/null +++ b/src/external/mavros_msgs/msg/CamIMUStamp.msg @@ -0,0 +1,4 @@ +# IMU-Camera synchronisation data + +builtin_interfaces/Time frame_stamp # Timestamp when the camera was triggered +int32 frame_seq_id # Sequence number of the image frame diff --git a/src/external/mavros_msgs/msg/CameraImageCaptured.msg b/src/external/mavros_msgs/msg/CameraImageCaptured.msg new file mode 100644 index 0000000..44c1a98 --- /dev/null +++ b/src/external/mavros_msgs/msg/CameraImageCaptured.msg @@ -0,0 +1,11 @@ +# MAVLink message: CAMERA_IMAGE_CAPTURED +# https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED + +std_msgs/Header header + +geometry_msgs/Quaternion orientation # Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) +geographic_msgs/GeoPoint geo +float32 relative_alt # mm Altitude above ground +int32 image_index # Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) +int8 capture_result # Boolean indicating success (1) or failure (0) while capturing this image. +string file_url #URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. diff --git a/src/external/mavros_msgs/msg/CellularStatus.msg b/src/external/mavros_msgs/msg/CellularStatus.msg new file mode 100644 index 0000000..2c9f99f --- /dev/null +++ b/src/external/mavros_msgs/msg/CellularStatus.msg @@ -0,0 +1,9 @@ +#Follows https://mavlink.io/en/messages/common.html#CELLULAR_STATUS specification + +uint8 status +uint8 failure_reason +uint8 type +uint8 quality +uint16 mcc +uint16 mnc +uint16 lac \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/CommandCode.msg b/src/external/mavros_msgs/msg/CommandCode.msg new file mode 100644 index 0000000..00fd7da --- /dev/null +++ b/src/external/mavros_msgs/msg/CommandCode.msg @@ -0,0 +1,200 @@ +# MAV_CMD command codes. +# Actual meaning and params you may find in MAVLink documentation +# https://mavlink.io/en/messages/common.html#MAV_CMD + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum_mav_cmd() +# ]]] +# MAV_CMD_AIRFRAME +uint16 AIRFRAME_CONFIGURATION = 2520 + +# MAV_CMD_ARM +uint16 ARM_AUTHORIZATION_REQUEST = 3001 # Request authorization to arm the vehicle to a external entity, the arm authorizer is responsible to request all data that is needs from the vehicle before authorize or deny the request. If approved the progress of command_ack message should be set with period of time that this authorization is valid in seconds or in case it was denied it should be set with one of the reasons in ARM_AUTH_DENIED_REASON. + +# MAV_CMD_CAMERA +uint16 CAMERA_TRACK_POINT = 2004 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. +uint16 CAMERA_TRACK_RECTANGLE = 2005 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. +uint16 CAMERA_STOP_TRACKING = 2010 # Stops ongoing tracking. + +# MAV_CMD_CAN +uint16 CAN_FORWARD = 32000 # Request forwarding of CAN packets from the given CAN bus to this interface. CAN Frames are sent using CAN_FRAME and CANFD_FRAME messages + +# MAV_CMD_COMPONENT +uint16 COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component + +# MAV_CMD_CONDITION +uint16 CONDITION_DELAY = 112 # Delay mission state machine. +uint16 CONDITION_CHANGE_ALT = 113 # Ascend/descend to target altitude at specified rate. Delay mission state machine until desired altitude reached. +uint16 CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. +uint16 CONDITION_YAW = 115 # Reach a certain target angle. +uint16 CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + +# MAV_CMD_CONTROL +uint16 CONTROL_HIGH_LATENCY = 2600 # Request to start/stop transmitting over the high latency telemetry + +# MAV_CMD_DO +uint16 DO_FOLLOW = 32 # Begin following a target +uint16 DO_FOLLOW_REPOSITION = 33 # Reposition the MAV after a follow target command has been sent +uint16 DO_SET_MODE = 176 # Set system mode. +uint16 DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times +uint16 DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points +uint16 DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. +uint16 DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. +uint16 DO_SET_RELAY = 181 # Set a relay to a condition. +uint16 DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period. +uint16 DO_SET_SERVO = 183 # Set a servo to a desired PWM value. +uint16 DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. +uint16 DO_FLIGHTTERMINATION = 185 # Terminate flight immediately +uint16 DO_CHANGE_ALTITUDE = 186 # Change altitude set point. +uint16 DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude/Altitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. +uint16 DO_RALLY_LAND = 190 # Mission command to perform a landing from a rally point. +uint16 DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. +uint16 DO_REPOSITION = 192 # Reposition the vehicle to a specific WGS84 global position. +uint16 DO_PAUSE_CONTINUE = 193 # If in a GPS controlled position mode, hold the current position or continue. +uint16 DO_SET_REVERSE = 194 # Set moving direction to forward or reverse. +uint16 DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_SET_ROI_SYSID = 198 # Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. +uint16 DO_CONTROL_VIDEO = 200 # Control onboard camera system. +uint16 DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 DO_DIGICAM_CONFIGURE = 202 # Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). +uint16 DO_DIGICAM_CONTROL = 203 # Control digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). +uint16 DO_MOUNT_CONFIGURE = 204 # Mission command to configure a camera or antenna mount +uint16 DO_MOUNT_CONTROL = 205 # Mission command to control a camera or antenna mount +uint16 DO_SET_CAM_TRIGG_DIST = 206 # Mission command to set camera trigger distance for this flight. The camera is triggered each time this distance is exceeded. This command can also be used to set the shutter integration time for the camera. +uint16 DO_FENCE_ENABLE = 207 # Mission command to enable the geofence +uint16 DO_PARACHUTE = 208 # Mission item/command to release a parachute or enable/disable auto release. +uint16 DO_MOTOR_TEST = 209 # Mission command to perform motor test. +uint16 DO_INVERTED_FLIGHT = 210 # Change to/from inverted flight. +uint16 DO_GRIPPER = 211 # Mission command to operate a gripper. +uint16 DO_AUTOTUNE_ENABLE = 212 # Enable/disable autotune. +uint16 DO_SET_CAM_TRIGG_INTERVAL = 214 # Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. +uint16 DO_MOUNT_CONTROL_QUAT = 220 # Mission command to control a camera or antenna mount, using a quaternion as reference. +uint16 DO_GUIDED_MASTER = 221 # set id of master controller +uint16 DO_GUIDED_LIMITS = 222 # Set limits for external control +uint16 DO_ENGINE_CONTROL = 223 # Control vehicle engine. This is interpreted by the vehicles engine controller to change the target engine state. It is intended for vehicles with internal combustion engines +uint16 DO_SET_MISSION_CURRENT = 224 # Set the mission item with sequence number seq as current item. This means that the MAV will continue to this mission item on the shortest path (not following the mission items in-between). +uint16 DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration +uint16 DO_JUMP_TAG = 601 # Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. +uint16 DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Set gimbal manager pitch/yaw setpoints (low rate command). It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: only the gimbal manager will react to this command - it will be ignored by a gimbal device. Use GIMBAL_MANAGER_SET_PITCHYAW if you need to stream pitch/yaw setpoints at higher rate. +uint16 DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control. +uint16 DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system. +uint16 DO_VTOL_TRANSITION = 3000 # Request VTOL transition +uint16 DO_ADSB_OUT_IDENT = 10001 # Trigger the start of an ADSB-out IDENT. This should only be used when requested to do so by an Air Traffic Controller in controlled airspace. This starts the IDENT which is then typically held for 18 seconds by the hardware per the Mode A, C, and S transponder spec. +uint16 DO_WINCH = 42600 # Command to operate winch. + +# MAV_CMD_FIXED +uint16 FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. + +# MAV_CMD_GET +uint16 GET_HOME_POSITION = 410 # Request the home position from the vehicle. +uint16 GET_MESSAGE_INTERVAL = 510 # Request the interval between messages for a particular MAVLink message ID. The receiver should ACK the command and then emit its response in a MESSAGE_INTERVAL message. + +# MAV_CMD_IMAGE +uint16 IMAGE_START_CAPTURE = 2000 # Start image capture sequence. CAMERA_IMAGE_CAPTURED must be emitted after each capture. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param 1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. +uint16 IMAGE_STOP_CAPTURE = 2001 # Stop image capture sequence. Param1 (id) may be used to specify the target camera: 0: all cameras, 1 to 6: autopilot-connected cameras, 7-255: MAVLink camera component ID. It is needed in order to target specific cameras connected to the autopilot, or specific sensors in a multi-sensor camera (neither of which have a distinct MAVLink component ID). It is also needed to specify the target camera in missions. When used in a mission, an autopilot should execute the MAV_CMD for a specified local camera (param1 = 1-6), or resend it as a command if it is intended for a MAVLink camera (param1 = 7 - 255), setting the command's target_component as the param1 value (and setting param1 in the command to zero). If the param1 is 0 the autopilot should do both. When sent in a command the target MAVLink address is set using target_component. If addressed specifically to an autopilot: param1 should be used in the same way as it is for missions (though command should NACK with MAV_RESULT_DENIED if a specified local camera does not exist). If addressed to a MAVLink camera, param1 can be used to address all cameras (0), or to separately address 1 to 7 individual sensors. Other values should be NACKed with MAV_RESULT_DENIED. If the command is broadcast (target_component is 0) then param 1 should be set to 0 (any other value should be NACKED with MAV_RESULT_DENIED). An autopilot would trigger any local cameras and forward the command to all channels. + +# MAV_CMD_JUMP +uint16 JUMP_TAG = 600 # Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. + +# MAV_CMD_LOGGING +uint16 LOGGING_START = 2510 # Request to start streaming logging data over MAVLink (see also LOGGING_DATA message) +uint16 LOGGING_STOP = 2511 # Request to stop streaming log data over MAVLink + +# MAV_CMD_MISSION +uint16 MISSION_START = 300 # start running a mission + +# MAV_CMD_NAV +uint16 NAV_WAYPOINT = 16 # Navigate to waypoint. +uint16 NAV_LOITER_UNLIM = 17 # Loiter around this waypoint an unlimited amount of time +uint16 NAV_LOITER_TURNS = 18 # Loiter around this waypoint for X turns +uint16 NAV_LOITER_TIME = 19 # Loiter around this waypoint for X seconds +uint16 NAV_RETURN_TO_LAUNCH = 20 # Return to launch location +uint16 NAV_LAND = 21 # Land at location. +uint16 NAV_TAKEOFF = 22 # Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. +uint16 NAV_LAND_LOCAL = 23 # Land at local position (local frame only) +uint16 NAV_TAKEOFF_LOCAL = 24 # Takeoff from local position (local frame only) +uint16 NAV_FOLLOW = 25 # Vehicle following, i.e. this waypoint represents the position of a moving vehicle +uint16 NAV_CONTINUE_AND_CHANGE_ALT = 30 # Continue on the current course and climb/descend to specified altitude. When the altitude is reached continue to the next command (i.e., don't proceed to the next command until the desired altitude is reached. +uint16 NAV_LOITER_TO_ALT = 31 # Begin loiter at the specified Latitude and Longitude. If Lat=Lon=0, then loiter at the current position. Don't consider the navigation command complete (don't leave loiter) until the altitude has been reached. Additionally, if the Heading Required parameter is non-zero the aircraft will not leave the loiter until heading toward the next waypoint. +uint16 NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. +uint16 NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. +uint16 NAV_SPLINE_WAYPOINT = 82 # Navigate to waypoint using a spline path. +uint16 NAV_VTOL_TAKEOFF = 84 # Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). +uint16 NAV_VTOL_LAND = 85 # Land using VTOL mode +uint16 NAV_GUIDED_ENABLE = 92 # hand control over to an external controller +uint16 NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time +uint16 NAV_PAYLOAD_PLACE = 94 # Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. +uint16 NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration +uint16 NAV_SET_YAW_SPEED = 213 # Sets a desired vehicle turn angle and speed change. +uint16 NAV_FENCE_RETURN_POINT = 5000 # Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. +uint16 NAV_FENCE_POLYGON_VERTEX_INCLUSION = 5001 # Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. +uint16 NAV_FENCE_POLYGON_VERTEX_EXCLUSION = 5002 # Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. +uint16 NAV_FENCE_CIRCLE_INCLUSION = 5003 # Circular fence area. The vehicle must stay inside this area. +uint16 NAV_FENCE_CIRCLE_EXCLUSION = 5004 # Circular fence area. The vehicle must stay outside this area. +uint16 NAV_RALLY_POINT = 5100 # Rally point. You can have multiple rally points defined. + +# MAV_CMD_OBLIQUE +uint16 OBLIQUE_SURVEY = 260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. + +# MAV_CMD_OVERRIDE +uint16 OVERRIDE_GOTO = 252 # Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. + +# MAV_CMD_PANORAMA +uint16 PANORAMA_CREATE = 2800 # Create a panorama at the current position + +# MAV_CMD_PAYLOAD +uint16 PAYLOAD_PREPARE_DEPLOY = 30001 # Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. +uint16 PAYLOAD_CONTROL_DEPLOY = 30002 # Control the payload deployment. + +# MAV_CMD_PREFLIGHT +uint16 PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. +uint16 PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. +uint16 PREFLIGHT_UAVCAN = 243 # Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). +uint16 PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. +uint16 PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. + +# MAV_CMD_REQUEST +uint16 REQUEST_MESSAGE = 512 # Request the target system(s) emit a single instance of a specified message (i.e. a "one-shot" version of MAV_CMD_SET_MESSAGE_INTERVAL). +uint16 REQUEST_PROTOCOL_VERSION = 519 # Request MAVLink protocol version compatibility. All receivers should ACK the command and then emit their capabilities in an PROTOCOL_VERSION message +uint16 REQUEST_AUTOPILOT_CAPABILITIES = 520 # Request autopilot capabilities. The receiver should ACK the command and then emit its capabilities in an AUTOPILOT_VERSION message +uint16 REQUEST_CAMERA_INFORMATION = 521 # Request camera information (CAMERA_INFORMATION). +uint16 REQUEST_CAMERA_SETTINGS = 522 # Request camera settings (CAMERA_SETTINGS). +uint16 REQUEST_STORAGE_INFORMATION = 525 # Request storage information (STORAGE_INFORMATION). Use the command's target_component to target a specific component's storage. +uint16 REQUEST_CAMERA_CAPTURE_STATUS = 527 # Request camera capture status (CAMERA_CAPTURE_STATUS) +uint16 REQUEST_FLIGHT_INFORMATION = 528 # Request flight information (FLIGHT_INFORMATION) +uint16 REQUEST_VIDEO_STREAM_INFORMATION = 2504 # Request video stream information (VIDEO_STREAM_INFORMATION) +uint16 REQUEST_VIDEO_STREAM_STATUS = 2505 # Request video stream status (VIDEO_STREAM_STATUS) + +# MAV_CMD_RESET +uint16 RESET_CAMERA_SETTINGS = 529 # Reset all camera settings to Factory Default + +# MAV_CMD_RUN +uint16 RUN_PREARM_CHECKS = 401 # Instructs system to run pre-arm checks. This command should return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed, otherwse MAV_RESULT_ACCEPTED. Note that the return value from executing this command does not indicate whether the vehicle is armable or not, just whether the system has successfully run/is currently running the checks. The result of the checks is reflected in the SYS_STATUS message. + +# MAV_CMD_SET +uint16 SET_MESSAGE_INTERVAL = 511 # Set the interval between messages for a particular MAVLink message ID. This interface replaces REQUEST_DATA_STREAM. +uint16 SET_CAMERA_MODE = 530 # Set camera running mode. Use NaN for reserved values. GCS will send a MAV_CMD_REQUEST_VIDEO_STREAM_STATUS command after a mode change if the camera supports video streaming. +uint16 SET_CAMERA_ZOOM = 531 # Set camera zoom. Camera must respond with a CAMERA_SETTINGS message (on success). +uint16 SET_CAMERA_FOCUS = 532 # Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). +uint16 SET_GUIDED_SUBMODE_STANDARD = 4000 # This command sets the submode to standard guided when vehicle is in guided mode. The vehicle holds position and altitude and the user can input the desired velocities along all three axes. +uint16 SET_GUIDED_SUBMODE_CIRCLE = 4001 # This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. + +# MAV_CMD_START +uint16 START_RX_PAIR = 500 # Starts receiver pairing. + +# MAV_CMD_STORAGE +uint16 STORAGE_FORMAT = 526 # Format a storage medium. Once format is complete, a STORAGE_INFORMATION message is sent. Use the command's target_component to target a specific component's storage. + +# MAV_CMD_UAVCAN +uint16 UAVCAN_GET_NODE_INFO = 5200 # Commands the vehicle to respond with a sequence of messages UAVCAN_NODE_INFO, one message per every UAVCAN node that is online. Note that some of the response messages can be lost, which the receiver can detect easily by checking whether every received UAVCAN_NODE_STATUS has a matching message UAVCAN_NODE_INFO received earlier; if not, this command should be sent again in order to request re-transmission of the node information messages. + +# MAV_CMD_VIDEO +uint16 VIDEO_START_CAPTURE = 2500 # Starts video capture (recording). +uint16 VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture (recording). +uint16 VIDEO_START_STREAMING = 2502 # Start video streaming +uint16 VIDEO_STOP_STREAMING = 2503 # Stop the given video stream + +# [[[end]]] (checksum: 73ee94ac661c9fcb61528a6668f71d94) diff --git a/src/external/mavros_msgs/msg/CompanionProcessStatus.msg b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg new file mode 100644 index 0000000..ea8b08c --- /dev/null +++ b/src/external/mavros_msgs/msg/CompanionProcessStatus.msg @@ -0,0 +1,19 @@ +# Mavros message: COMPANIONPROCESSSTATUS + +std_msgs/Header header + +uint8 state # See enum COMPANION_PROCESS_STATE +uint8 component # See enum MAV_COMPONENT + +uint8 MAV_STATE_UNINIT = 0 +uint8 MAV_STATE_BOOT = 1 +uint8 MAV_STATE_CALIBRATING = 2 +uint8 MAV_STATE_STANDBY = 3 +uint8 MAV_STATE_ACTIVE = 4 +uint8 MAV_STATE_CRITICAL = 5 +uint8 MAV_STATE_EMERGENCY = 6 +uint8 MAV_STATE_POWEROFF = 7 +uint8 MAV_STATE_FLIGHT_TERMINATION = 8 + +uint8 MAV_COMP_ID_OBSTACLE_AVOIDANCE = 196 +uint8 MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY = 197 diff --git a/src/external/mavros_msgs/msg/DebugValue.msg b/src/external/mavros_msgs/msg/DebugValue.msg new file mode 100644 index 0000000..286b6c2 --- /dev/null +++ b/src/external/mavros_msgs/msg/DebugValue.msg @@ -0,0 +1,26 @@ +# Msg for Debug MAVLink API +# +# Supported types: +# DEBUG https://mavlink.io/en/messages/common.html#DEBUG +# DEBUG_VECTOR https://mavlink.io/en/messages/common.html#DEBUG_VECT +# DEBUG_FLOAT_ARRAY https://mavlink.io/en/messages/common.html#DEBUG_FLOAT_ARRAY +# NAMED_VALUE_FLOAT https://mavlink.io/en/messages/common.html#NAMED_VALUE_FLOAT +# NAMED_VALUE_INT https://mavlink.io/en/messages/common.html#NAMED_VALUE_INT + +std_msgs/Header header + +int32 index # index value of DEBUG value (-1 if not indexed) +int32 array_id # Unique ID used to discriminate between DEBUG_FLOAT_ARRAYS (-1 if not used) + +string name # value name/key + +float32 value_float # float value for NAMED_VALUE_FLOAT and DEBUG +int32 value_int # int value for NAMED_VALUE_INT +float32[] data # DEBUG vector or array + +uint8 type +uint8 TYPE_DEBUG = 0 +uint8 TYPE_DEBUG_VECT = 1 +uint8 TYPE_DEBUG_FLOAT_ARRAY = 2 +uint8 TYPE_NAMED_VALUE_FLOAT = 3 +uint8 TYPE_NAMED_VALUE_INT = 4 diff --git a/src/external/mavros_msgs/msg/ESCInfo.msg b/src/external/mavros_msgs/msg/ESCInfo.msg new file mode 100644 index 0000000..71bcc41 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCInfo.msg @@ -0,0 +1,14 @@ +# ESCInfo.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_INFO + +std_msgs/Header header + +uint16 counter +uint8 count +uint8 connection_type +uint8 info +mavros_msgs/ESCInfoItem[] esc_info + diff --git a/src/external/mavros_msgs/msg/ESCInfoItem.msg b/src/external/mavros_msgs/msg/ESCInfoItem.msg new file mode 100644 index 0000000..fc63856 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCInfoItem.msg @@ -0,0 +1,12 @@ +# ESCInfoItem.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_INFO + +std_msgs/Header header + +uint16 failure_flags +uint32 error_count +int32 temperature + diff --git a/src/external/mavros_msgs/msg/ESCStatus.msg b/src/external/mavros_msgs/msg/ESCStatus.msg new file mode 100644 index 0000000..870579b --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCStatus.msg @@ -0,0 +1,9 @@ +# ESCStatus.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_STATUS + +std_msgs/Header header + +mavros_msgs/ESCStatusItem[] esc_status diff --git a/src/external/mavros_msgs/msg/ESCStatusItem.msg b/src/external/mavros_msgs/msg/ESCStatusItem.msg new file mode 100644 index 0000000..252aa0b --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCStatusItem.msg @@ -0,0 +1,11 @@ +# ESCStatusItem.msg +# +# +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#ESC_STATUS + +std_msgs/Header header + +int32 rpm +float32 voltage +float32 current diff --git a/src/external/mavros_msgs/msg/ESCTelemetry.msg b/src/external/mavros_msgs/msg/ESCTelemetry.msg new file mode 100644 index 0000000..f0c5916 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCTelemetry.msg @@ -0,0 +1,10 @@ +# APM ESC Telemetry as returned by BLHeli +# +# See: +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12 + +std_msgs/Header header + +mavros_msgs/ESCTelemetryItem[] esc_telemetry diff --git a/src/external/mavros_msgs/msg/ESCTelemetryItem.msg b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg new file mode 100644 index 0000000..e5c0c17 --- /dev/null +++ b/src/external/mavros_msgs/msg/ESCTelemetryItem.msg @@ -0,0 +1,15 @@ +# APM ESC Telemetry as returned by BLHeli +# +# See: +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_1_TO_4 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_5_TO_8 +# https://mavlink.io/en/messages/ardupilotmega.html#ESC_TELEMETRY_9_TO_12 + +std_msgs/Header header + +float32 temperature # deg C +float32 voltage # V +float32 current # A +float32 totalcurrent # Ah +int32 rpm # 1/min +uint16 count # count of telemetry packets diff --git a/src/external/mavros_msgs/msg/EstimatorStatus.msg b/src/external/mavros_msgs/msg/EstimatorStatus.msg new file mode 100644 index 0000000..40101b8 --- /dev/null +++ b/src/external/mavros_msgs/msg/EstimatorStatus.msg @@ -0,0 +1,23 @@ +# Current autopilot estimator state +# +# https://mavlink.io/en/messages/common.html#ESTIMATOR_STATUS_FLAGS + +std_msgs/Header header +bool attitude_status_flag + +bool velocity_horiz_status_flag +bool velocity_vert_status_flag + +bool pos_horiz_rel_status_flag +bool pos_horiz_abs_status_flag + +bool pos_vert_abs_status_flag +bool pos_vert_agl_status_flag + +bool const_pos_mode_status_flag + +bool pred_pos_horiz_rel_status_flag +bool pred_pos_horiz_abs_status_flag + +bool gps_glitch_status_flag +bool accel_error_status_flag \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/ExtendedState.msg b/src/external/mavros_msgs/msg/ExtendedState.msg new file mode 100644 index 0000000..47c5c29 --- /dev/null +++ b/src/external/mavros_msgs/msg/ExtendedState.msg @@ -0,0 +1,19 @@ +# Extended autopilot state +# +# https://mavlink.io/en/messages/common.html#EXTENDED_SYS_STATE + +uint8 VTOL_STATE_UNDEFINED = 0 +uint8 VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VTOL_STATE_MC = 3 +uint8 VTOL_STATE_FW = 4 + +uint8 LANDED_STATE_UNDEFINED = 0 +uint8 LANDED_STATE_ON_GROUND = 1 +uint8 LANDED_STATE_IN_AIR = 2 +uint8 LANDED_STATE_TAKEOFF = 3 +uint8 LANDED_STATE_LANDING = 4 + +std_msgs/Header header +uint8 vtol_state +uint8 landed_state diff --git a/src/external/mavros_msgs/msg/FileEntry.msg b/src/external/mavros_msgs/msg/FileEntry.msg new file mode 100644 index 0000000..e733326 --- /dev/null +++ b/src/external/mavros_msgs/msg/FileEntry.msg @@ -0,0 +1,12 @@ +# File/Dir information + +uint8 TYPE_FILE = 0 +uint8 TYPE_DIRECTORY = 1 + +string name +uint8 type +uint64 size + +# Not supported by MAVLink FTP +#builtin_interfaces/Time atime +#int32 access_flags diff --git a/src/external/mavros_msgs/msg/GPSINPUT.msg b/src/external/mavros_msgs/msg/GPSINPUT.msg new file mode 100644 index 0000000..15a038a --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSINPUT.msg @@ -0,0 +1,37 @@ +# FCU GPS INPUT message for the gps_input plugin +# mavlink GPS_INPUT message. + +std_msgs/Header header +## GPS_FIX_TYPE enum +uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected +uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected +uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position +uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position +uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position +uint8 GPS_FIX_TYPE_RTK_FLOATR = 5 # TK float, 3D position +uint8 GPS_FIX_TYPE_RTK_FIXEDR = 6 # TK Fixed, 3D position +uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations +uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position +uint8 fix_type # [GPS_FIX_TYPE] GPS fix type + +uint8 gps_id # ID of the GPS for multiple GPS inputs +uint16 ignore_flags # Bitmap indicating which GPS input flags fields to ignore. All other fields must be provided. + +uint32 time_week_ms # [ms] GPS time (from start of GPS week) +uint16 time_week # GPS week number +int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid) +int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid) +float32 alt # [m] Altitude (MSL). Positive for up. + +float32 hdop # [m] GPS HDOP horizontal dilution of position. +float32 vdop # [m] GPS VDOP vertical dilution of position +float32 vn # [m/s] GPS velocity in NORTH direction in earth-fixed NED frame +float32 ve # [m/s] GPS velocity in EAST direction in earth-fixed NED frame +float32 vd # [m/s] GPS velocity in DOWN direction in earth-fixed NED frame + +float32 speed_accuracy # [m/s] GPS speed accuracy +float32 horiz_accuracy # [m] GPS horizontal accuracy +float32 vert_accuracy # [m] GPS vertical accuracy + +uint8 satellites_visible # Number of satellites visible. If unknown, set to 255 +uint16 yaw # [cdeg] Yaw in earth frame from north. diff --git a/src/external/mavros_msgs/msg/GPSRAW.msg b/src/external/mavros_msgs/msg/GPSRAW.msg new file mode 100644 index 0000000..5b754b3 --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSRAW.msg @@ -0,0 +1,37 @@ +# FCU GPS RAW message for the gps_status plugin +# A merge of mavlink GPS_RAW_INT and +# mavlink GPS2_RAW messages. + +std_msgs/Header header +## GPS_FIX_TYPE enum +uint8 GPS_FIX_TYPE_NO_GPS = 0 # No GPS connected +uint8 GPS_FIX_TYPE_NO_FIX = 1 # No position information, GPS is connected +uint8 GPS_FIX_TYPE_2D_FIX = 2 # 2D position +uint8 GPS_FIX_TYPE_3D_FIX = 3 # 3D position +uint8 GPS_FIX_TYPE_DGPS = 4 # DGPS/SBAS aided 3D position +uint8 GPS_FIX_TYPE_RTK_FLOAT = 5 # RTK float, 3D position +uint8 GPS_FIX_TYPE_RTK_FIXED = 6 # RTK Fixed, 3D position +uint8 GPS_FIX_TYPE_STATIC = 7 # Static fixed, typically used for base stations +uint8 GPS_FIX_TYPE_PPP = 8 # PPP, 3D position +uint8 fix_type # [GPS_FIX_TYPE] GPS fix type + +int32 lat # [degE7] Latitude (WGS84, EGM96 ellipsoid) +int32 lon # [degE7] Longitude (WGS84, EGM96 ellipsoid) +int32 alt # [mm] Altitude (MSL). Positive for up. Note that virtually all GPS modules provide the MSL altitude in addition to the WGS84 altitude. +uint16 eph # GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX +uint16 epv # GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX +uint16 vel # [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX +uint16 cog # [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX +uint8 satellites_visible # Number of satellites visible. If unknown, set to 255 + +# -*- only available with MAVLink v2.0 and GPS_RAW_INT messages -*- +int32 alt_ellipsoid # [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. +uint32 h_acc # [mm] Position uncertainty. Positive for up. +uint32 v_acc # [mm] Altitude uncertainty. Positive for up. +uint32 vel_acc # [mm] Speed uncertainty. Positive for up. +int32 hdg_acc # [degE5] Heading / track uncertainty +uint16 yaw # [cdeg] Yaw in earth frame from north. + +# -*- only available with MAVLink v2.0 and GPS2_RAW messages -*- +uint8 dgps_numch # Number of DGPS satellites +uint32 dgps_age # [ms] Age of DGPS info diff --git a/src/external/mavros_msgs/msg/GPSRTK.msg b/src/external/mavros_msgs/msg/GPSRTK.msg new file mode 100644 index 0000000..2cac818 --- /dev/null +++ b/src/external/mavros_msgs/msg/GPSRTK.msg @@ -0,0 +1,18 @@ +# FCU GPS RTK message for the gps_status plugin +# A copy of mavlink GPS_RTK message + +std_msgs/Header header + +uint8 rtk_receiver_id # Identification of connected RTK receiver. +int16 wn # GPS Week Number of last baseline. +uint32 tow # [ms] GPS Time of Week of last baseline. +uint8 rtk_health # GPS-specific health report for RTK data. +uint8 rtk_rate # [Hz] Rate of baseline messages being received by GPS. +uint8 nsats # Current number of sats used for RTK calculation. +int32 baseline_a # [mm] Current baseline in ECEF x or NED north component, depends on header.frame_id. +int32 baseline_b # [mm] Current baseline in ECEF y or NED east component, depends on header.frame_id. +int32 baseline_c # [mm] Current baseline in ECEF z or NED down component, depends on header.frame_id. +uint32 accuracy # Current estimate of baseline accuracy. +int32 iar_num_hypotheses # Current number of integer ambiguity hypotheses. + + diff --git a/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg new file mode 100644 index 0000000..612375d --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceAttitudeStatus.msg @@ -0,0 +1,32 @@ +# MAVLink message: GIMBAL_DEVICE_ATTITUDE_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_ATTITUDE_STATUS + +std_msgs/Header header + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint16 flags # Current gimbal flags set (bitwise) - See GIMBAL_DEVICE_FLAGS +#GIMBAL_DEVICE_FLAGS +uint16 FLAGS_RETRACT = 1 # Set to retracted safe position (no stabilization), takes presedence over all other flags. +uint16 FLAGS_NEUTRAL = 2 # Set to neutral/default position, taking precedence over all other flags except RETRACT. Neutral is commonly forward-facing and horizontal (pitch=yaw=0) but may be any orientation. +uint16 FLAGS_ROLL_LOCK = 4 # Lock roll angle to absolute angle relative to horizon (not relative to drone). This is generally the default with a stabilizing gimbal. +uint16 FLAGS_PITCH_LOCK = 8 # Lock pitch angle to absolute angle relative to horizon (not relative to drone). This is generally the default. +uint16 FLAGS_YAW_LOCK = 16 # Lock yaw angle to absolute angle relative to North (not relative to drone). If this flag is set, the quaternion is in the Earth frame with the x-axis pointing North (yaw absolute). If this flag is not set, the quaternion frame is in the Earth frame rotated so that the x-axis is pointing forward (yaw relative to vehicle). + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity (NaN if unknown) +float32 angular_velocity_y # Y component of angular velocity (NaN if unknown) +float32 angular_velocity_z # Z component of angular velocity (NaN if unknown) + +uint32 failure_flags # Failure flags (0 for no failure) (bitwise) - See GIMBAL_DEVICE_ERROR_FLAGS +#GIMBAL_DEVICE_ERROR_FLAGS +uint32 ERROR_FLAGS_AT_ROLL_LIMIT = 1 # Gimbal device is limited by hardware roll limit. +uint32 ERROR_FLAGS_AT_PITCH_LIMIT = 2 # Gimbal device is limited by hardware pitch limit. +uint32 ERROR_FLAGS_AT_YAW_LIMIT = 4 # Gimbal device is limited by hardware yaw limit. +uint32 ERROR_FLAGS_ENCODER_ERROR = 8 # There is an error with the gimbal encoders. +uint32 ERROR_FLAGS_POWER_ERROR = 16 # There is an error with the gimbal power source. +uint32 ERROR_FLAGS_MOTOR_ERROR = 32 # There is an error with the gimbal motor's. +uint32 ERROR_FLAGS_SOFTWARE_ERROR = 64 # There is an error with the gimbal's software. +uint32 ERROR_FLAGS_COMMS_ERROR = 128 # There is an error with the gimbal's communication. +uint32 ERROR_FLAGS_CALIBRATION_RUNNING = 256 # Gimbal is currently calibrating. diff --git a/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg new file mode 100644 index 0000000..0d68309 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceInformation.msg @@ -0,0 +1,34 @@ +# MAVLink message: GIMBAL_DEVICE_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +std_msgs/Header header + +string vendor_name # Name of the gimbal vendor. +string model_name # Name of the gimbal model. +string custom_name # Custom name of the gimbal given to it by the user. +uint32 firmware_version # Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +uint32 hardware_version # Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). +uint64 uid # UID of gimbal hardware (0 if unknown). + +uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_DEVICE_CAP_FLAGS +#GIMBAL_DEVICE_CAP_FLAGS +uint32 CAP_FLAGS_HAS_RETRACT = 1 # Gimbal device supports a retracted position +uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Gimbal device supports a horizontal, forward looking position, stabilized +uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Gimbal device supports rotating around roll axis. +uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Gimbal device supports to follow a roll angle relative to the vehicle +uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Gimbal device supports locking to an roll angle (generally that's the default with roll stabilized) +uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Gimbal device supports rotating around pitch axis. +uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Gimbal device supports to follow a pitch angle relative to the vehicle +uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Gimbal device supports locking to an pitch angle (generally that's the default with pitch stabilized) +uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Gimbal device supports rotating around yaw axis. +uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Gimbal device supports to follow a yaw angle relative to the vehicle (generally that's the default) +uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Gimbal device supports locking to an absolute heading (often this is an option available) +uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Gimbal device supports yawing/panning infinetely (e.g. using slip disk). + +uint16 custom_cap_flags # Bitmap for use for gimbal-specific capability flags. +float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 pitch_min # Minimum pitch angle (positive: up, negative: down) +float32 pitch_max # Maximum pitch angle (positive: up, negative: down) +float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left) +float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg new file mode 100644 index 0000000..fdbc73d --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalDeviceSetAttitude.msg @@ -0,0 +1,18 @@ +# MAVLink message: GIMBAL_DEVICE_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_SET_ATTITUDE + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint16 flags # Low level gimbal flags (bitwise) - See GIMBAL_DEVICE_FLAGS +#GIMBAL_DEVICE_FLAGS +uint16 FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint16 FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint16 FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint16 FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint16 FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerInformation.msg b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg new file mode 100644 index 0000000..38dbc88 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerInformation.msg @@ -0,0 +1,29 @@ +# MAVLink message: GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION + +std_msgs/Header header + +uint32 cap_flags # Bitmap of gimbal capability flags - see GIMBAL_MANAGER_CAP_FLAGS +#GIMBAL_MANAGER_CAP_FLAGS +uint32 CAP_FLAGS_HAS_RETRACT = 1 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT. +uint32 CAP_FLAGS_HAS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL. +uint32 CAP_FLAGS_HAS_ROLL_AXIS = 4 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS. +uint32 CAP_FLAGS_HAS_ROLL_FOLLOW = 8 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW. +uint32 CAP_FLAGS_HAS_ROLL_LOCK = 16 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK. +uint32 CAP_FLAGS_HAS_PITCH_AXIS = 32 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS. +uint32 CAP_FLAGS_HAS_PITCH_FOLLOW = 64 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW. +uint32 CAP_FLAGS_HAS_PITCH_LOCK = 128 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK. +uint32 CAP_FLAGS_HAS_YAW_AXIS = 256 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS. +uint32 CAP_FLAGS_HAS_YAW_FOLLOW = 512 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW. +uint32 CAP_FLAGS_HAS_YAW_LOCK = 1024 # Based on GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK. +uint32 CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 # Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. +uint32 CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 # Gimbal manager supports to point to a local position. +uint32 CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 # Gimbal manager supports to point to a global latitude, longitude, altitude position. + +uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for. +float32 roll_min # Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 roll_max # Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) +float32 pitch_min # Minimum pitch angle (positive: up, negative: down) +float32 pitch_max # Maximum pitch angle (positive: up, negative: down) +float32 yaw_min # Minimum yaw angle (positive: to the right, negative: to the left) +float32 yaw_max # Maximum yaw angle (positive: to the right, negative: to the left) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg new file mode 100644 index 0000000..8bbec37 --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerSetAttitude.msg @@ -0,0 +1,24 @@ +# MAVLink message: GIMBAL_MANAGER_SET_ATTITUDE +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_ATTITUDE + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use (bitwise) - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +geometry_msgs/Quaternion q # Quaternion, x, y, z, w (0 0 0 1 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) +float32 angular_velocity_x # X component of angular velocity, positive is rolling to the right, NaN to be ignored. +float32 angular_velocity_y # Y component of angular velocity, positive is pitching up, NaN to be ignored. +float32 angular_velocity_z # Z component of angular velocity, positive is yawing to the right, NaN to be ignored. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg new file mode 100644 index 0000000..e87874e --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerSetPitchyaw.msg @@ -0,0 +1,27 @@ +# MAVLink message: GIMBAL_MANAGER_SET_PITCHYAW +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_PITCHYAW +# Note that this message structure is identical also to GIMBAL_MANAGER_SET_MANUAL_CONTROL and is +# reused as such by the plugin +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_SET_MANUAL_CONTROL + +uint8 target_system # System ID +uint8 target_component # Component ID + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +float32 pitch # Pitch angle (positive: up, negative: down, NaN to be ignored). +float32 yaw # Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). +float32 pitch_rate # Pitch angular rate (positive: up, negative: down, NaN to be ignored). +float32 yaw_rate # Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GimbalManagerStatus.msg b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg new file mode 100644 index 0000000..8df8b3e --- /dev/null +++ b/src/external/mavros_msgs/msg/GimbalManagerStatus.msg @@ -0,0 +1,19 @@ +# MAVLink message: GIMBAL_MANAGER_STATUS +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_STATUS + +std_msgs/Header header + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Gimbal device ID that this gimbal manager is responsible for. + +uint8 sysid_primary # System ID of MAVLink component with primary control, 0 for none. +uint8 compid_primary # Component ID of MAVLink component with primary control, 0 for none. +uint8 sysid_secondary # System ID of MAVLink component with secondary control, 0 for none. +uint8 compid_secondary # Component ID of MAVLink component with secondary control, 0 for none. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/GlobalPositionTarget.msg b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg new file mode 100644 index 0000000..445a1a7 --- /dev/null +++ b/src/external/mavros_msgs/msg/GlobalPositionTarget.msg @@ -0,0 +1,34 @@ +# Message for SET_POSITION_TARGET_GLOBAL_INT +# +# https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT +# Some complex system requires all feautures that mavlink +# message provide. See issue #402, #415. + +std_msgs/Header header + +uint8 coordinate_frame +uint8 FRAME_GLOBAL_INT = 5 +uint8 FRAME_GLOBAL_REL_ALT = 6 +uint8 FRAME_GLOBAL_TERRAIN_ALT = 11 + +uint16 type_mask +uint16 IGNORE_LATITUDE = 1 # Position ignore flags +uint16 IGNORE_LONGITUDE = 2 +uint16 IGNORE_ALTITUDE = 4 +uint16 IGNORE_VX = 8 # Velocity vector ignore flags +uint16 IGNORE_VY = 16 +uint16 IGNORE_VZ = 32 +uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags +uint16 IGNORE_AFY = 128 +uint16 IGNORE_AFZ = 256 +uint16 FORCE = 512 # Force in af vector flag +uint16 IGNORE_YAW = 1024 +uint16 IGNORE_YAW_RATE = 2048 + +float64 latitude +float64 longitude +float32 altitude # in meters, AMSL or above terrain +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration_or_force +float32 yaw +float32 yaw_rate diff --git a/src/external/mavros_msgs/msg/HilActuatorControls.msg b/src/external/mavros_msgs/msg/HilActuatorControls.msg new file mode 100644 index 0000000..755384f --- /dev/null +++ b/src/external/mavros_msgs/msg/HilActuatorControls.msg @@ -0,0 +1,10 @@ +# HilActuatorControls.msg +# +# ROS representation of MAVLink HIL_ACTUATOR_CONTROLS +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_ACTUATOR_CONTROLS + +std_msgs/Header header +float32[16] controls +uint8 mode +uint64 flags diff --git a/src/external/mavros_msgs/msg/HilControls.msg b/src/external/mavros_msgs/msg/HilControls.msg new file mode 100644 index 0000000..a953aff --- /dev/null +++ b/src/external/mavros_msgs/msg/HilControls.msg @@ -0,0 +1,18 @@ +# HilControls.msg +# +# ROS representation of MAVLink HIL_CONTROLS +# (deprecated, use HIL_ACTUATOR_CONTROLS instead) +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_CONTROLS + +std_msgs/Header header +float32 roll_ailerons +float32 pitch_elevator +float32 yaw_rudder +float32 throttle +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +uint8 mode +uint8 nav_mode diff --git a/src/external/mavros_msgs/msg/HilGPS.msg b/src/external/mavros_msgs/msg/HilGPS.msg new file mode 100644 index 0000000..d74c204 --- /dev/null +++ b/src/external/mavros_msgs/msg/HilGPS.msg @@ -0,0 +1,17 @@ +# HilControls.msg +# +# ROS representation of MAVLink HIL_GPS +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_GPS + +std_msgs/Header header +uint8 fix_type +geographic_msgs/GeoPoint geo +uint16 eph +uint16 epv +uint16 vel +int16 vn +int16 ve +int16 vd +uint16 cog +uint8 satellites_visible diff --git a/src/external/mavros_msgs/msg/HilSensor.msg b/src/external/mavros_msgs/msg/HilSensor.msg new file mode 100644 index 0000000..c7ceccb --- /dev/null +++ b/src/external/mavros_msgs/msg/HilSensor.msg @@ -0,0 +1,16 @@ +# HilSensor.msg +# +# ROS representation of MAVLink HIL_SENSOR +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_SENSOR + +std_msgs/Header header + +geometry_msgs/Vector3 acc +geometry_msgs/Vector3 gyro +geometry_msgs/Vector3 mag +float32 abs_pressure +float32 diff_pressure +float32 pressure_alt +float32 temperature +uint32 fields_updated diff --git a/src/external/mavros_msgs/msg/HilStateQuaternion.msg b/src/external/mavros_msgs/msg/HilStateQuaternion.msg new file mode 100644 index 0000000..980db1e --- /dev/null +++ b/src/external/mavros_msgs/msg/HilStateQuaternion.msg @@ -0,0 +1,15 @@ +# HilStateQuaternion.msg +# +# ROS representation of MAVLink HIL_STATE_QUATERNION +# See mavlink message documentation here: +# https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION + +std_msgs/Header header + +geometry_msgs/Quaternion orientation +geometry_msgs/Vector3 angular_velocity +geometry_msgs/Vector3 linear_acceleration +geometry_msgs/Vector3 linear_velocity +geographic_msgs/GeoPoint geo +float32 ind_airspeed +float32 true_airspeed diff --git a/src/external/mavros_msgs/msg/HomePosition.msg b/src/external/mavros_msgs/msg/HomePosition.msg new file mode 100644 index 0000000..f73bb51 --- /dev/null +++ b/src/external/mavros_msgs/msg/HomePosition.msg @@ -0,0 +1,10 @@ +# MAVLink message: HOME_POSITION +# https://mavlink.io/en/messages/common.html#HOME_POSITION + +std_msgs/Header header + +geographic_msgs/GeoPoint geo # geodetic coordinates in WGS-84 datum + +geometry_msgs/Point position # local position +geometry_msgs/Quaternion orientation # XXX: verify field name (q[4]) +geometry_msgs/Vector3 approach # position of the end of approach vector diff --git a/src/external/mavros_msgs/msg/LandingTarget.msg b/src/external/mavros_msgs/msg/LandingTarget.msg new file mode 100644 index 0000000..d5e6bd0 --- /dev/null +++ b/src/external/mavros_msgs/msg/LandingTarget.msg @@ -0,0 +1,32 @@ +# MAVLink message: LANDING_TARGET +# https://mavlink.io/en/messages/common.html + +std_msgs/Header header + +## MAV_FRAME enum +uint8 GLOBAL = 0 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL) +uint8 LOCAL_NED = 2 # Local coordinate frame, Z-up (x: north, y: east, z: down). +uint8 MISSION = 3 # NOT a coordinate frame, indicates a mission command. +uint8 GLOBAL_RELATIVE_ALT = 4 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 LOCAL_ENU = 5 # Local coordinate frame, Z-down (x: east, y: north, z: up) +uint8 GLOBAL_INT = 6 # Global coordinate frame, WGS84 coordinate system. First value / x: latitude in degrees*1.0e-7, second value / y: longitude in degrees*1.0e-7, third value / z: positive altitude over mean sea level (MSL) +uint8 GLOBAL_RELATIVE_ALT_INT = 7 # Global coordinate frame, WGS84 coordinate system, relative altitude over ground with respect to the home position. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 LOCAL_OFFSET_NED = 8 # Offset to the current local frame. Anything expressed in this frame should be added to the current local frame position. +uint8 BODY_NED = 9 # Setpoint in body NED frame. This makes sense if all position control is externalized - e.g. useful to command 2 m/s^2 acceleration to the right. +uint8 BODY_OFFSET_NED = 10 # Offset in body NED frame. This makes sense if adding setpoints to the current flight path, to avoid an obstacle - e.g. useful to command 2 m/s^2 acceleration to the east. +uint8 GLOBAL_TERRAIN_ALT = 11 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 GLOBAL_TERRAIN_ALT_INT = 12 # Global coordinate frame with above terrain level altitude. WGS84 coordinate system, relative altitude over terrain with respect to the waypoint coordinate. First value / x: latitude in degrees*10e-7, second value / y: longitude in degrees*10e-7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. + +## LANDING_TARGET_TYPE enum +uint8 LIGHT_BEACON = 0 # Landing target signaled by light beacon (ex: IR-LOCK) +uint8 RADIO_BEACON = 1 # Landing target signaled by radio beacon (ex: ILS, NDB) +uint8 VISION_FIDUCIAL = 2 # Landing target represented by a fiducial marker (ex: ARTag) +uint8 VISION_OTHER = 3 # Landing target represented by a pre-defined visual shape/feature (ex: X-marker, H-marker, square) + +uint8 target_num +uint8 frame +float32[2] angle +float32 distance +float32[2] size +geometry_msgs/Pose pose +uint8 type diff --git a/src/external/mavros_msgs/msg/LogData.msg b/src/external/mavros_msgs/msg/LogData.msg new file mode 100644 index 0000000..829a0a3 --- /dev/null +++ b/src/external/mavros_msgs/msg/LogData.msg @@ -0,0 +1,11 @@ +# Reply to LogRequestData, - a chunk of a log +# +# :id: - log id +# :offset: - offset into the log +# :data: - chunk of data (if zero-sized, then there are no more chunks) + +std_msgs/Header header + +uint16 id +uint32 offset +uint8[] data diff --git a/src/external/mavros_msgs/msg/LogEntry.msg b/src/external/mavros_msgs/msg/LogEntry.msg new file mode 100644 index 0000000..b212686 --- /dev/null +++ b/src/external/mavros_msgs/msg/LogEntry.msg @@ -0,0 +1,15 @@ +# Information about a single log +# +# :id: - log id +# :num_logs: - total number of logs +# :last_log_num: - id of last log +# :time_utc: - UTC timestamp of log (ros::Time(0) if not available) +# :size: - size of log in bytes (may be approximate) + +std_msgs/Header header + +uint16 id +uint16 num_logs +uint16 last_log_num +builtin_interfaces/Time time_utc +uint32 size diff --git a/src/external/mavros_msgs/msg/MagnetometerReporter.msg b/src/external/mavros_msgs/msg/MagnetometerReporter.msg new file mode 100644 index 0000000..593f5f6 --- /dev/null +++ b/src/external/mavros_msgs/msg/MagnetometerReporter.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +uint8 report +float32 confidence \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/ManualControl.msg b/src/external/mavros_msgs/msg/ManualControl.msg new file mode 100644 index 0000000..f6db210 --- /dev/null +++ b/src/external/mavros_msgs/msg/ManualControl.msg @@ -0,0 +1,7 @@ +# Manual Control state +std_msgs/Header header +float32 x +float32 y +float32 z +float32 r +uint16 buttons diff --git a/src/external/mavros_msgs/msg/Mavlink.msg b/src/external/mavros_msgs/msg/Mavlink.msg new file mode 100644 index 0000000..afb80ed --- /dev/null +++ b/src/external/mavros_msgs/msg/Mavlink.msg @@ -0,0 +1,38 @@ +# Mavlink message transport type. +# +# Used to transport mavlink_message_t via ROS topic +# +# :framing_status: +# Frame decoding status: OK, CRC error, bad Signature (mavlink v2.0) +# You may simply drop all non valid messages. +# Used for GCS Bridge to transport unknown messages. +# +# :magic: +# STX byte, used to determine protocol version v1.0 or v2.0. +# +# Please use mavros_msgs::mavlink::convert() from +# to convert between ROS and MAVLink message type + +# mavlink_framing_t enum +uint8 FRAMING_OK = 1 +uint8 FRAMING_BAD_CRC = 2 +uint8 FRAMING_BAD_SIGNATURE = 3 + +# stx values +uint8 MAVLINK_V10 = 254 +uint8 MAVLINK_V20 = 253 + +std_msgs/Header header +uint8 framing_status + +uint8 magic # STX byte +uint8 len +uint8 incompat_flags +uint8 compat_flags +uint8 seq +uint8 sysid +uint8 compid +uint32 msgid # 24-bit message id +uint16 checksum +uint64[<=33] payload64 # max size: (255+2+7)/8 +uint8[<=13] signature # optional signature, max size: 13 diff --git a/src/external/mavros_msgs/msg/MountControl.msg b/src/external/mavros_msgs/msg/MountControl.msg new file mode 100644 index 0000000..0c2ee72 --- /dev/null +++ b/src/external/mavros_msgs/msg/MountControl.msg @@ -0,0 +1,18 @@ +# MAVLink message: DO_MOUNT_CONTROL +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL + +std_msgs/Header header + +uint8 mode # See enum MAV_MOUNT_MODE. +uint8 MAV_MOUNT_MODE_RETRACT = 0 +uint8 MAV_MOUNT_MODE_NEUTRAL = 1 +uint8 MAV_MOUNT_MODE_MAVLINK_TARGETING = 2 +uint8 MAV_MOUNT_MODE_RC_TARGETING = 3 +uint8 MAV_MOUNT_MODE_GPS_POINT = 4 + +float32 pitch # pitch degrees or degrees/second depending on mount mode. +float32 roll # roll degrees or degrees/second depending on mount mode. +float32 yaw # yaw degrees or degrees/second depending on mount mode. +float32 altitude # altitude depending on mount mode. +float32 latitude # latitude in degrees * 1E7, set if appropriate mount mode. +float32 longitude # longitude in degrees * 1E7, set if appropriate mount mode. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/NavControllerOutput.msg b/src/external/mavros_msgs/msg/NavControllerOutput.msg new file mode 100644 index 0000000..d5f8f6d --- /dev/null +++ b/src/external/mavros_msgs/msg/NavControllerOutput.msg @@ -0,0 +1,12 @@ +# https://mavlink.io/en/messages/common.html#NAV_CONTROLLER_OUTPUT + +std_msgs/Header header + +float32 nav_roll # Current desired roll +float32 nav_pitch # Current desired pitch +int16 nav_bearing # Current desired heading +int16 target_bearing # Bearing to current waypoint/target +uint16 wp_dist # Distance to active waypoint +float32 alt_error # Current altitude error +float32 aspd_error # Current airspeed error +float32 xtrack_error # Current crosstrack error on x-y plane \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/OnboardComputerStatus.msg b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg new file mode 100644 index 0000000..d6d06b0 --- /dev/null +++ b/src/external/mavros_msgs/msg/OnboardComputerStatus.msg @@ -0,0 +1,25 @@ +# Mavros message: ONBOARDCOMPUTERSTATUS + +std_msgs/Header header + +uint8 component # See enum MAV_COMPONENT + +uint32 uptime # [ms] Time since system boot +uint8 type # Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. +uint8[8] cpu_cores # CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. +uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused +uint8[4] gpu_cores # GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused +uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. +int8 temperature_board # [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. +int8[8] temperature_core # [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. +int16[4] fan_speed # [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. +uint32 ram_usage # [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. +uint32 ram_total # [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. +uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. +uint32[4] storage_usage # [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. +uint32[4] storage_total # [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_type # Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary. +uint32[6] link_tx_rate # [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_rx_rate # [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_tx_max # [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. +uint32[6] link_rx_max # [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/OpticalFlow.msg b/src/external/mavros_msgs/msg/OpticalFlow.msg new file mode 100644 index 0000000..db857b8 --- /dev/null +++ b/src/external/mavros_msgs/msg/OpticalFlow.msg @@ -0,0 +1,9 @@ +# OPTICAL_FLOW message data + +std_msgs/Header header + +geometry_msgs/Vector3 flow +geometry_msgs/Vector3 flow_comp_m +uint8 quality +float32 ground_distance +geometry_msgs/Vector3 flow_rate diff --git a/src/external/mavros_msgs/msg/OpticalFlowRad.msg b/src/external/mavros_msgs/msg/OpticalFlowRad.msg new file mode 100644 index 0000000..f773fac --- /dev/null +++ b/src/external/mavros_msgs/msg/OpticalFlowRad.msg @@ -0,0 +1,14 @@ +# OPTICAL_FLOW_RAD message data + +std_msgs/Header header + +uint32 integration_time_us +float32 integrated_x +float32 integrated_y +float32 integrated_xgyro +float32 integrated_ygyro +float32 integrated_zgyro +int16 temperature +uint8 quality +uint32 time_delta_distance_us +float32 distance diff --git a/src/external/mavros_msgs/msg/OverrideRCIn.msg b/src/external/mavros_msgs/msg/OverrideRCIn.msg new file mode 100644 index 0000000..81bc040 --- /dev/null +++ b/src/external/mavros_msgs/msg/OverrideRCIn.msg @@ -0,0 +1,9 @@ +# Override RC Input +# Currently MAVLink defines override for 18 channels + +# https://mavlink.io/en/messages/common.html#RC_CHANNELS_OVERRIDE + +uint16 CHAN_RELEASE=0 +uint16 CHAN_NOCHANGE=65535 + +uint16[18] channels diff --git a/src/external/mavros_msgs/msg/Param.msg b/src/external/mavros_msgs/msg/Param.msg new file mode 100644 index 0000000..f2ccfaa --- /dev/null +++ b/src/external/mavros_msgs/msg/Param.msg @@ -0,0 +1,11 @@ +# Parameter msg. + +# XXX DEPRECATED: replaced by ParamEvent + +std_msgs/Header header + +string param_id +mavros_msgs/ParamValue value + +uint16 param_index +uint16 param_count diff --git a/src/external/mavros_msgs/msg/ParamEvent.msg b/src/external/mavros_msgs/msg/ParamEvent.msg new file mode 100644 index 0000000..493df0a --- /dev/null +++ b/src/external/mavros_msgs/msg/ParamEvent.msg @@ -0,0 +1,14 @@ +# Parameter Event +# +# That messages replaces mavros_msgs/Param from mavros v1. +# Reason for that: ROS2 have native message for parameters +# +# ROS2 also have it's own ParameterEvent stream, which could be used +# to get FCU updates too. But that message is simpler to use. + +std_msgs/Header header + +string param_id +rcl_interfaces/ParameterValue value +uint16 param_index +uint16 param_count diff --git a/src/external/mavros_msgs/msg/ParamValue.msg b/src/external/mavros_msgs/msg/ParamValue.msg new file mode 100644 index 0000000..4a98639 --- /dev/null +++ b/src/external/mavros_msgs/msg/ParamValue.msg @@ -0,0 +1,12 @@ +# Parameter value storage type. +# +# Integer and float fields: +# +# if integer != 0: it is integer value +# else if real != 0.0: it is float value +# else: it is zero. + +# XXX DEPRECATED: replaced by rmw_interfaces/ParameterValue + +int64 integer +float64 real diff --git a/src/external/mavros_msgs/msg/PlayTuneV2.msg b/src/external/mavros_msgs/msg/PlayTuneV2.msg new file mode 100644 index 0000000..d92a976 --- /dev/null +++ b/src/external/mavros_msgs/msg/PlayTuneV2.msg @@ -0,0 +1,10 @@ +# Play tune V2 +# +# https://mavlink.io/en/messages/common.html#PLAY_TUNE_V2 + +## TUNE_FORMAT enum +uint8 QBASIC1_1 = 1 +uint8 MML_MODERN = 2 + +uint8 format +string tune diff --git a/src/external/mavros_msgs/msg/PositionTarget.msg b/src/external/mavros_msgs/msg/PositionTarget.msg new file mode 100644 index 0000000..680be87 --- /dev/null +++ b/src/external/mavros_msgs/msg/PositionTarget.msg @@ -0,0 +1,32 @@ +# Message for SET_POSITION_TARGET_LOCAL_NED +# +# Some complex system requires all feautures that mavlink +# message provide. See issue #402. + +std_msgs/Header header + +uint8 coordinate_frame +uint8 FRAME_LOCAL_NED = 1 +uint8 FRAME_LOCAL_OFFSET_NED = 7 +uint8 FRAME_BODY_NED = 8 +uint8 FRAME_BODY_OFFSET_NED = 9 + +uint16 type_mask +uint16 IGNORE_PX = 1 # Position ignore flags +uint16 IGNORE_PY = 2 +uint16 IGNORE_PZ = 4 +uint16 IGNORE_VX = 8 # Velocity vector ignore flags +uint16 IGNORE_VY = 16 +uint16 IGNORE_VZ = 32 +uint16 IGNORE_AFX = 64 # Acceleration/Force vector ignore flags +uint16 IGNORE_AFY = 128 +uint16 IGNORE_AFZ = 256 +uint16 FORCE = 512 # Force in af vector flag +uint16 IGNORE_YAW = 1024 +uint16 IGNORE_YAW_RATE = 2048 + +geometry_msgs/Point position +geometry_msgs/Vector3 velocity +geometry_msgs/Vector3 acceleration_or_force +float32 yaw +float32 yaw_rate diff --git a/src/external/mavros_msgs/msg/RCIn.msg b/src/external/mavros_msgs/msg/RCIn.msg new file mode 100644 index 0000000..2a38d9c --- /dev/null +++ b/src/external/mavros_msgs/msg/RCIn.msg @@ -0,0 +1,5 @@ +# RAW RC input state + +std_msgs/Header header +uint8 rssi +uint16[] channels diff --git a/src/external/mavros_msgs/msg/RCOut.msg b/src/external/mavros_msgs/msg/RCOut.msg new file mode 100644 index 0000000..5ba3d15 --- /dev/null +++ b/src/external/mavros_msgs/msg/RCOut.msg @@ -0,0 +1,4 @@ +# RAW Servo out state + +std_msgs/Header header +uint16[] channels diff --git a/src/external/mavros_msgs/msg/RTCM.msg b/src/external/mavros_msgs/msg/RTCM.msg new file mode 100644 index 0000000..8c74498 --- /dev/null +++ b/src/external/mavros_msgs/msg/RTCM.msg @@ -0,0 +1,6 @@ +# RTCM message for the gps_rtk plugin +# The gps_rtk plugin will fragment the data if necessary and +# forward it to the FCU via Mavlink through the available link. +# data should be <= 4*180, higher will be discarded. +std_msgs/Header header +uint8[] data diff --git a/src/external/mavros_msgs/msg/RTKBaseline.msg b/src/external/mavros_msgs/msg/RTKBaseline.msg new file mode 100644 index 0000000..cbbd386 --- /dev/null +++ b/src/external/mavros_msgs/msg/RTKBaseline.msg @@ -0,0 +1,23 @@ +# RTKBaseline received from the FCU. +# Full description: https://mavlink.io/en/messages/common.html#GPS_RTK +# Mavlink Common, #127and #128 + +std_msgs/Header header + +uint32 time_last_baseline_ms +uint8 rtk_receiver_id +uint16 wn +uint32 tow +uint8 rtk_health +uint8 rtk_rate +uint8 nsats + +uint8 baseline_coords_type +uint8 RTK_BASELINE_COORDINATE_SYSTEM_ECEF = 0 # Earth-centered, earth-fixed +uint8 RTK_BASELINE_COORDINATE_SYSTEM_NED = 1 # RTK basestation centered, north, east, down + +int32 baseline_a_mm +int32 baseline_b_mm +int32 baseline_c_mm +uint32 accuracy +int32 iar_num_hypotheses diff --git a/src/external/mavros_msgs/msg/RadioStatus.msg b/src/external/mavros_msgs/msg/RadioStatus.msg new file mode 100644 index 0000000..dc7d3b4 --- /dev/null +++ b/src/external/mavros_msgs/msg/RadioStatus.msg @@ -0,0 +1,16 @@ +# RADIO_STATUS message + +std_msgs/Header header + +# message data +uint8 rssi +uint8 remrssi +uint8 txbuf +uint8 noise +uint8 remnoise +uint16 rxerrors +uint16 fixed + +# calculated +float32 rssi_dbm +float32 remrssi_dbm diff --git a/src/external/mavros_msgs/msg/State.msg b/src/external/mavros_msgs/msg/State.msg new file mode 100644 index 0000000..db00886 --- /dev/null +++ b/src/external/mavros_msgs/msg/State.msg @@ -0,0 +1,82 @@ +# Current autopilot state +# +# Known modes listed here: +# http://wiki.ros.org/mavros/CustomModes +# +# For system_status values +# see https://mavlink.io/en/messages/common.html#MAV_STATE +# + +std_msgs/Header header +bool connected +bool armed +bool guided +bool manual_input +string mode +uint8 system_status + +string MODE_APM_PLANE_MANUAL = MANUAL +string MODE_APM_PLANE_CIRCLE = CIRCLE +string MODE_APM_PLANE_STABILIZE = STABILIZE +string MODE_APM_PLANE_TRAINING = TRAINING +string MODE_APM_PLANE_ACRO = ACRO +string MODE_APM_PLANE_FBWA = FBWA +string MODE_APM_PLANE_FBWB = FBWB +string MODE_APM_PLANE_CRUISE = CRUISE +string MODE_APM_PLANE_AUTOTUNE = AUTOTUNE +string MODE_APM_PLANE_AUTO = AUTO +string MODE_APM_PLANE_RTL = RTL +string MODE_APM_PLANE_LOITER = LOITER +string MODE_APM_PLANE_LAND = LAND +string MODE_APM_PLANE_GUIDED = GUIDED +string MODE_APM_PLANE_INITIALISING = INITIALISING +string MODE_APM_PLANE_QSTABILIZE = QSTABILIZE +string MODE_APM_PLANE_QHOVER = QHOVER +string MODE_APM_PLANE_QLOITER = QLOITER +string MODE_APM_PLANE_QLAND = QLAND +string MODE_APM_PLANE_QRTL = QRTL + +string MODE_APM_COPTER_STABILIZE = STABILIZE +string MODE_APM_COPTER_ACRO = ACRO +string MODE_APM_COPTER_ALT_HOLD = ALT_HOLD +string MODE_APM_COPTER_AUTO = AUTO +string MODE_APM_COPTER_GUIDED = GUIDED +string MODE_APM_COPTER_LOITER = LOITER +string MODE_APM_COPTER_RTL = RTL +string MODE_APM_COPTER_CIRCLE = CIRCLE +string MODE_APM_COPTER_POSITION = POSITION +string MODE_APM_COPTER_LAND = LAND +string MODE_APM_COPTER_OF_LOITER = OF_LOITER +string MODE_APM_COPTER_DRIFT = DRIFT +string MODE_APM_COPTER_SPORT = SPORT +string MODE_APM_COPTER_FLIP = FLIP +string MODE_APM_COPTER_AUTOTUNE = AUTOTUNE +string MODE_APM_COPTER_POSHOLD = POSHOLD +string MODE_APM_COPTER_BRAKE = BRAKE +string MODE_APM_COPTER_THROW = THROW +string MODE_APM_COPTER_AVOID_ADSB = AVOID_ADSB +string MODE_APM_COPTER_GUIDED_NOGPS = GUIDED_NOGPS + +string MODE_APM_ROVER_MANUAL = MANUAL +string MODE_APM_ROVER_LEARNING = LEARNING +string MODE_APM_ROVER_STEERING = STEERING +string MODE_APM_ROVER_HOLD = HOLD +string MODE_APM_ROVER_AUTO = AUTO +string MODE_APM_ROVER_RTL = RTL +string MODE_APM_ROVER_GUIDED = GUIDED +string MODE_APM_ROVER_INITIALISING = INITIALISING + +string MODE_PX4_MANUAL = MANUAL +string MODE_PX4_ACRO = ACRO +string MODE_PX4_ALTITUDE = ALTCTL +string MODE_PX4_POSITION = POSCTL +string MODE_PX4_OFFBOARD = OFFBOARD +string MODE_PX4_STABILIZED = STABILIZED +string MODE_PX4_RATTITUDE = RATTITUDE +string MODE_PX4_MISSION = AUTO.MISSION +string MODE_PX4_LOITER = AUTO.LOITER +string MODE_PX4_RTL = AUTO.RTL +string MODE_PX4_LAND = AUTO.LAND +string MODE_PX4_RTGS = AUTO.RTGS +string MODE_PX4_READY = AUTO.READY +string MODE_PX4_TAKEOFF = AUTO.TAKEOFF diff --git a/src/external/mavros_msgs/msg/StatusEvent.msg b/src/external/mavros_msgs/msg/StatusEvent.msg new file mode 100644 index 0000000..3f90aea --- /dev/null +++ b/src/external/mavros_msgs/msg/StatusEvent.msg @@ -0,0 +1,19 @@ +# EVENT message representation +# https://mavlink.io/en/messages/common.html#EVENT + +# Severity levels +uint8 EMERGENCY = 0 +uint8 ALERT = 1 +uint8 CRITICAL = 2 +uint8 ERROR = 3 +uint8 WARNING = 4 +uint8 NOTICE = 5 +uint8 INFO = 6 +uint8 DEBUG = 7 + +# Fields +std_msgs/Header header +uint8 severity +uint32 px4_id +uint8[40] arguments +uint16 sequence diff --git a/src/external/mavros_msgs/msg/StatusText.msg b/src/external/mavros_msgs/msg/StatusText.msg new file mode 100644 index 0000000..901b869 --- /dev/null +++ b/src/external/mavros_msgs/msg/StatusText.msg @@ -0,0 +1,17 @@ +# STATUSTEXT message representation +# https://mavlink.io/en/messages/common.html#STATUSTEXT + +# Severity levels +uint8 EMERGENCY = 0 +uint8 ALERT = 1 +uint8 CRITICAL = 2 +uint8 ERROR = 3 +uint8 WARNING = 4 +uint8 NOTICE = 5 +uint8 INFO = 6 +uint8 DEBUG = 7 + +# Fields +std_msgs/Header header +uint8 severity +string text diff --git a/src/external/mavros_msgs/msg/SysStatus.msg b/src/external/mavros_msgs/msg/SysStatus.msg new file mode 100644 index 0000000..551940a --- /dev/null +++ b/src/external/mavros_msgs/msg/SysStatus.msg @@ -0,0 +1,15 @@ +std_msgs/Header header + +uint32 sensors_present +uint32 sensors_enabled +uint32 sensors_health +uint16 load +uint16 voltage_battery +int16 current_battery +int8 battery_remaining +uint16 drop_rate_comm +uint16 errors_comm +uint16 errors_count1 +uint16 errors_count2 +uint16 errors_count3 +uint16 errors_count4 \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/TerrainReport.msg b/src/external/mavros_msgs/msg/TerrainReport.msg new file mode 100644 index 0000000..5c9efce --- /dev/null +++ b/src/external/mavros_msgs/msg/TerrainReport.msg @@ -0,0 +1,12 @@ +# Message for TERRAIN_REPORT +# https://mavlink.io/en/messages/common.html#TERRAIN_REPORT + +std_msgs/Header header + +float64 latitude +float64 longitude +uint16 spacing +float32 terrain_height # in meters, terrain height +float32 current_height # in meters, vehicle height above terrain +uint16 pending +uint16 loaded diff --git a/src/external/mavros_msgs/msg/Thrust.msg b/src/external/mavros_msgs/msg/Thrust.msg new file mode 100644 index 0000000..fa2fcd6 --- /dev/null +++ b/src/external/mavros_msgs/msg/Thrust.msg @@ -0,0 +1,5 @@ +# Thrust to send to the FCU + +std_msgs/Header header + +float32 thrust diff --git a/src/external/mavros_msgs/msg/TimesyncStatus.msg b/src/external/mavros_msgs/msg/TimesyncStatus.msg new file mode 100644 index 0000000..dbfbf30 --- /dev/null +++ b/src/external/mavros_msgs/msg/TimesyncStatus.msg @@ -0,0 +1,7 @@ +# Status of the MAVLink time synchroniser + +std_msgs/Header header +uint64 remote_timestamp_ns # remote system timestamp (nanoseconds) +int64 observed_offset_ns # raw time offset directly observed from this timesync packet (nanoseconds) +int64 estimated_offset_ns # smoothed time offset between companion system and Mavros (nanoseconds) +float32 round_trip_time_ms # round trip time of this timesync packet (milliseconds) \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/Trajectory.msg b/src/external/mavros_msgs/msg/Trajectory.msg new file mode 100644 index 0000000..c43f63a --- /dev/null +++ b/src/external/mavros_msgs/msg/Trajectory.msg @@ -0,0 +1,19 @@ +# MAVLink message: TRAJECTORY +# https://mavlink.io/en/messages/common.html#TRAJECTORY + +std_msgs/Header header + +uint8 type # See enum MAV_TRAJECTORY_REPRESENTATION. +uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 +uint8 MAV_TRAJECTORY_REPRESENTATION_BEZIER = 1 + +mavros_msgs/PositionTarget point_1 +mavros_msgs/PositionTarget point_2 +mavros_msgs/PositionTarget point_3 +mavros_msgs/PositionTarget point_4 +mavros_msgs/PositionTarget point_5 + +uint8[5] point_valid # States if respective point is valid. +uint16[5] command # MAV_CMD associated with each point. UINT16_MAX if unused. + +float32[5] time_horizon # if type MAV_TRAJECTORY_REPRESENTATION_BEZIER, it represents the time horizon for each point, otherwise set to NaN diff --git a/src/external/mavros_msgs/msg/Tunnel.msg b/src/external/mavros_msgs/msg/Tunnel.msg new file mode 100644 index 0000000..a04dcd1 --- /dev/null +++ b/src/external/mavros_msgs/msg/Tunnel.msg @@ -0,0 +1,27 @@ +# Tunnel +# +# https://mavlink.io/en/messages/common.html#TUNNEL + +uint8 target_system +uint8 target_component +uint16 payload_type +uint8 payload_length +uint8[128] payload + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('MAV_TUNNEL_PAYLOAD_TYPE', 'PAYLOAD_TYPE_', 16) +# ]]] +# MAV_TUNNEL_PAYLOAD_TYPE +uint16 PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown. +uint16 PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller. +uint16 PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller. +# [[[end]]] (checksum: 3327b212af02c2d47d940cd6de049624) diff --git a/src/external/mavros_msgs/msg/VehicleInfo.msg b/src/external/mavros_msgs/msg/VehicleInfo.msg new file mode 100644 index 0000000..ea80b16 --- /dev/null +++ b/src/external/mavros_msgs/msg/VehicleInfo.msg @@ -0,0 +1,31 @@ +# Vehicle Info msg + +std_msgs/Header header + +uint8 HAVE_INFO_HEARTBEAT = 1 +uint8 HAVE_INFO_AUTOPILOT_VERSION = 2 +uint8 available_info # Bitmap shows what info is available + +# Vehicle address +uint8 sysid # SYSTEM ID +uint8 compid # COMPONENT ID + +# -*- Heartbeat info -*- +uint8 autopilot # MAV_AUTOPILOT +uint8 type # MAV_TYPE +uint8 system_status # MAV_STATE +uint8 base_mode +uint32 custom_mode +string mode # MAV_MODE string +uint32 mode_id # MAV_MODE number + +# -*- Autopilot version -*- +uint64 capabilities # MAV_PROTOCOL_CAPABILITY +uint32 flight_sw_version # Firmware version number +uint32 middleware_sw_version # Middleware version number +uint32 os_sw_version # Operating system version number +uint32 board_version # HW / board version (last 8 bytes should be silicon ID, if any) +string flight_custom_version # Custom version field, commonly from the first 8 bytes of the git hash +uint16 vendor_id # ID of the board vendor +uint16 product_id # ID of the product +uint64 uid # UID if provided by hardware diff --git a/src/external/mavros_msgs/msg/VfrHud.msg b/src/external/mavros_msgs/msg/VfrHud.msg new file mode 100644 index 0000000..81d0ba2 --- /dev/null +++ b/src/external/mavros_msgs/msg/VfrHud.msg @@ -0,0 +1,11 @@ +# Metrics typically displayed on a HUD for fixed wing aircraft +# +# VFR_HUD message + +std_msgs/Header header +float32 airspeed # m/s +float32 groundspeed # m/s +int16 heading # degrees 0..360 +float32 throttle # normalized to 0.0..1.0 +float32 altitude # MSL +float32 climb # current climb rate m/s diff --git a/src/external/mavros_msgs/msg/Vibration.msg b/src/external/mavros_msgs/msg/Vibration.msg new file mode 100644 index 0000000..d9170d0 --- /dev/null +++ b/src/external/mavros_msgs/msg/Vibration.msg @@ -0,0 +1,7 @@ +# VIBRATION message data +# @description: Vibration levels and accelerometer clipping + +std_msgs/Header header + +geometry_msgs/Vector3 vibration # 3-axis vibration levels +float32[3] clipping # Accelerometers clipping \ No newline at end of file diff --git a/src/external/mavros_msgs/msg/Waypoint.msg b/src/external/mavros_msgs/msg/Waypoint.msg new file mode 100644 index 0000000..7afa95e --- /dev/null +++ b/src/external/mavros_msgs/msg/Waypoint.msg @@ -0,0 +1,45 @@ +# Waypoint.msg +# +# ROS representation of MAVLink MISSION_ITEM +# See mavlink documentation + + + +# see enum MAV_FRAME +uint8 frame +uint8 FRAME_GLOBAL = 0 +uint8 FRAME_LOCAL_NED = 1 +uint8 FRAME_MISSION = 2 +uint8 FRAME_GLOBAL_REL_ALT = 3 +uint8 FRAME_LOCAL_ENU = 4 +uint8 FRAME_GLOBAL_INT = 5 +uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 +uint8 FRAME_LOCAL_OFFSET_NED = 7 +uint8 FRAME_BODY_NED = 8 +uint8 FRAME_BODY_OFFSET_NED = 9 +uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 +uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 +uint8 FRAME_BODY_FRD = 12 +uint8 FRAME_RESERVED_13 = 13 +uint8 FRAME_RESERVED_14 = 14 +uint8 FRAME_RESERVED_15 = 15 +uint8 FRAME_RESERVED_16 = 16 +uint8 FRAME_RESERVED_17 = 17 +uint8 FRAME_RESERVED_18 = 18 +uint8 FRAME_RESERVED_19 = 19 +uint8 FRAME_LOCAL_FRD = 20 +uint8 FRAME_LOCAL_FLU = 21 + +# see enum MAV_CMD and CommandCode.msg +uint16 command + +bool is_current +bool autocontinue +# meaning of this params described in enum MAV_CMD +float32 param1 +float32 param2 +float32 param3 +float32 param4 +float64 x_lat +float64 y_long +float64 z_alt diff --git a/src/external/mavros_msgs/msg/WaypointList.msg b/src/external/mavros_msgs/msg/WaypointList.msg new file mode 100644 index 0000000..de4111e --- /dev/null +++ b/src/external/mavros_msgs/msg/WaypointList.msg @@ -0,0 +1,9 @@ +# WaypointList.msg +# +# :current_seq: seq nr of currently active waypoint +# waypoints[current_seq].is_current == True +# +# :waypoints: list of waypoints + +uint16 current_seq +mavros_msgs/Waypoint[] waypoints diff --git a/src/external/mavros_msgs/msg/WaypointReached.msg b/src/external/mavros_msgs/msg/WaypointReached.msg new file mode 100644 index 0000000..6fc36a9 --- /dev/null +++ b/src/external/mavros_msgs/msg/WaypointReached.msg @@ -0,0 +1,7 @@ +# That message represent MISSION_ITEM_REACHED +# +# :wp_seq: index number of reached waypoint + +std_msgs/Header header + +uint16 wp_seq diff --git a/src/external/mavros_msgs/msg/WheelOdomStamped.msg b/src/external/mavros_msgs/msg/WheelOdomStamped.msg new file mode 100644 index 0000000..782c3ab --- /dev/null +++ b/src/external/mavros_msgs/msg/WheelOdomStamped.msg @@ -0,0 +1,6 @@ +# Stamped wheel odometry message +# +# For streaming timestamped data from FCU wheel encoders (RPM or WHEEL_DISTANCE). + +std_msgs/Header header +float64[] data diff --git a/src/external/mavros_msgs/package.xml b/src/external/mavros_msgs/package.xml new file mode 100644 index 0000000..9c5d8bc --- /dev/null +++ b/src/external/mavros_msgs/package.xml @@ -0,0 +1,46 @@ + + + + mavros_msgs + 2.9.0 + + mavros_msgs defines messages for MAVROS. + + + Vladimir Ermakov + + GPLv3 + LGPLv3 + BSD + + http://wiki.ros.org/mavros_msgs + https://github.com/mavlink/mavros.git + https://github.com/mavlink/mavros/issues + + Vladimir Ermakov + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + + + rcl_interfaces + geographic_msgs + geometry_msgs + sensor_msgs + + + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/external/mavros_msgs/srv/CommandAck.srv b/src/external/mavros_msgs/srv/CommandAck.srv new file mode 100644 index 0000000..f1d708e --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandAck.srv @@ -0,0 +1,11 @@ +# Generic COMMAND_ACK + +uint16 command +uint8 result +uint8 progress +uint32 result_param2 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandBool.srv b/src/external/mavros_msgs/srv/CommandBool.srv new file mode 100644 index 0000000..54eab1f --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandBool.srv @@ -0,0 +1,6 @@ +# Common type for switch commands + +bool value +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandHome.srv b/src/external/mavros_msgs/srv/CommandHome.srv new file mode 100644 index 0000000..89882c7 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandHome.srv @@ -0,0 +1,10 @@ +# request set new home position + +bool current_gps +float32 yaw +float32 latitude +float32 longitude +float32 altitude +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandInt.srv b/src/external/mavros_msgs/srv/CommandInt.srv new file mode 100644 index 0000000..f2242c7 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandInt.srv @@ -0,0 +1,19 @@ +# Generic COMMAND_INT + +bool broadcast # send this command in broadcast mode + +uint8 frame +uint16 command +uint8 current +uint8 autocontinue +float32 param1 +float32 param2 +float32 param3 +float32 param4 +int32 x # latitude in deg * 1E7 or local x * 1E4 m +int32 y # longitude in deg * 1E7 or local y * 1E4 m +float32 z # altitude +--- +bool success +# seems that this message don't produce andy COMMAND_ACK messages +# so no result field diff --git a/src/external/mavros_msgs/srv/CommandLong.srv b/src/external/mavros_msgs/srv/CommandLong.srv new file mode 100644 index 0000000..ca7f2bb --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandLong.srv @@ -0,0 +1,17 @@ +# Generic COMMAND_LONG + +bool broadcast # send this command in broadcast mode + +uint16 command +uint8 confirmation +float32 param1 +float32 param2 +float32 param3 +float32 param4 +float32 param5 # x_lat +float32 param6 # y_lon +float32 param7 # z_alt +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTOL.srv b/src/external/mavros_msgs/srv/CommandTOL.srv new file mode 100644 index 0000000..b76c85e --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTOL.srv @@ -0,0 +1,10 @@ +# Common type for Take Off and Landing + +float32 min_pitch # used by takeoff +float32 yaw +float32 latitude +float32 longitude +float32 altitude +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTOLLocal.srv b/src/external/mavros_msgs/srv/CommandTOLLocal.srv new file mode 100644 index 0000000..33d522f --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTOLLocal.srv @@ -0,0 +1,10 @@ +#Common type for LOCAL Take Off and Landing + +float32 min_pitch # used by takeoff +float32 offset # used by land (landing position accuracy) +float32 rate # speed of takeoff/land in m/s +float32 yaw # in radians +geometry_msgs/Vector3 position #(x,y,z) in meters +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTriggerControl.srv b/src/external/mavros_msgs/srv/CommandTriggerControl.srv new file mode 100644 index 0000000..fa3977b --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTriggerControl.srv @@ -0,0 +1,8 @@ +# Type for controlling onboard camera triggering system + +bool trigger_enable # Trigger enable/disable +bool sequence_reset # Reset the trigger sequence +bool trigger_pause # Pause triggering, but without switching the camera off or retracting it. +--- +bool success +uint8 result diff --git a/src/external/mavros_msgs/srv/CommandTriggerInterval.srv b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv new file mode 100644 index 0000000..85af0f5 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandTriggerInterval.srv @@ -0,0 +1,7 @@ +# Type for controlling camera trigger interval and integration time + +float32 cycle_time # Trigger cycle_time (interval between to triggers) - set to 0 to ignore command +float32 integration_time # Camera shutter integration_time - set to 0 to ignore command +--- +bool success +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/CommandVtolTransition.srv b/src/external/mavros_msgs/srv/CommandVtolTransition.srv new file mode 100644 index 0000000..fce9a72 --- /dev/null +++ b/src/external/mavros_msgs/srv/CommandVtolTransition.srv @@ -0,0 +1,16 @@ + +# MAVLink command: DO_VTOL_TRANSITION +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_VTOL_TRANSITION + +std_msgs/Header header + +# MAV_VTOL_STATE +uint8 STATE_MC = 3 +uint8 STATE_FW = 4 + +uint8 state # See enum MAV_VTOL_STATE. + +--- +bool success +uint8 result # Raw result returned by COMMAND_ACK + \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/EndpointAdd.srv b/src/external/mavros_msgs/srv/EndpointAdd.srv new file mode 100644 index 0000000..c102adb --- /dev/null +++ b/src/external/mavros_msgs/srv/EndpointAdd.srv @@ -0,0 +1,14 @@ +# +# Adds endpoint to router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +string url # mavconn URL or topic prefix for TYPE_UAS +uint8 type # should be set to one of the TYPE_xxx +--- +bool successful # true if endpoint added and opened +string reason # returns error description if open fails +uint32 id # ID of new endpoint, should be > 0 if endpoint created diff --git a/src/external/mavros_msgs/srv/EndpointDel.srv b/src/external/mavros_msgs/srv/EndpointDel.srv new file mode 100644 index 0000000..04d1ff3 --- /dev/null +++ b/src/external/mavros_msgs/srv/EndpointDel.srv @@ -0,0 +1,17 @@ +# +# Removes endpoint from router +# + +uint8 TYPE_FCU = 0 +uint8 TYPE_GCS = 1 +uint8 TYPE_UAS = 2 + +# delete by ID, leave 0 for second option +uint32 id + +# delete by url+type pair +string url +uint8 type + +--- +bool successful diff --git a/src/external/mavros_msgs/srv/FileChecksum.srv b/src/external/mavros_msgs/srv/FileChecksum.srv new file mode 100644 index 0000000..75cb4be --- /dev/null +++ b/src/external/mavros_msgs/srv/FileChecksum.srv @@ -0,0 +1,12 @@ +# FTP::Checksum +# +# :file_path: file to calculate checksum +# :crc32: file checksum +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +uint32 crc32 +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileClose.srv b/src/external/mavros_msgs/srv/FileClose.srv new file mode 100644 index 0000000..b99419e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileClose.srv @@ -0,0 +1,10 @@ +# FTP::Close +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileList.srv b/src/external/mavros_msgs/srv/FileList.srv new file mode 100644 index 0000000..d589a9e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileList.srv @@ -0,0 +1,10 @@ +# FTP::List +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +mavros_msgs/FileEntry[] list +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileMakeDir.srv b/src/external/mavros_msgs/srv/FileMakeDir.srv new file mode 100644 index 0000000..c2067d9 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileMakeDir.srv @@ -0,0 +1,9 @@ +# FTP::MakeDir +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileOpen.srv b/src/external/mavros_msgs/srv/FileOpen.srv new file mode 100644 index 0000000..9ad26c3 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileOpen.srv @@ -0,0 +1,17 @@ +# FTP::Open +# +# :file_path: used as session id in read/write/close services +# :size: file size returned for MODE_READ +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +uint8 MODE_READ = 0 # open for read +uint8 MODE_WRITE = 1 # open for write +uint8 MODE_CREATE = 2 # do creat() + +string file_path +uint8 mode +--- +uint32 size +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRead.srv b/src/external/mavros_msgs/srv/FileRead.srv new file mode 100644 index 0000000..d8aa592 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRead.srv @@ -0,0 +1,13 @@ +# FTP::Read +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 offset +uint64 size +--- +uint8[] data +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRemove.srv b/src/external/mavros_msgs/srv/FileRemove.srv new file mode 100644 index 0000000..af0328f --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRemove.srv @@ -0,0 +1,9 @@ +# FTP::Remove +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRemoveDir.srv b/src/external/mavros_msgs/srv/FileRemoveDir.srv new file mode 100644 index 0000000..c95d52f --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRemoveDir.srv @@ -0,0 +1,9 @@ +# FTP::RemoveDir +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string dir_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileRename.srv b/src/external/mavros_msgs/srv/FileRename.srv new file mode 100644 index 0000000..0e367a5 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileRename.srv @@ -0,0 +1,10 @@ +# FTP::Rename +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string old_path +string new_path +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileTruncate.srv b/src/external/mavros_msgs/srv/FileTruncate.srv new file mode 100644 index 0000000..4c78596 --- /dev/null +++ b/src/external/mavros_msgs/srv/FileTruncate.srv @@ -0,0 +1,10 @@ +# FTP::Truncate +# +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 length +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/FileWrite.srv b/src/external/mavros_msgs/srv/FileWrite.srv new file mode 100644 index 0000000..d94815e --- /dev/null +++ b/src/external/mavros_msgs/srv/FileWrite.srv @@ -0,0 +1,12 @@ +# FTP::Write +# +# Call FTP::Open first. +# :success: indicates success end of request +# :r_errno: remote errno if applicapable + +string file_path +uint64 offset +uint8[] data +--- +bool success +int32 r_errno diff --git a/src/external/mavros_msgs/srv/GimbalGetInformation.srv b/src/external/mavros_msgs/srv/GimbalGetInformation.srv new file mode 100644 index 0000000..c91bda2 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalGetInformation.srv @@ -0,0 +1,10 @@ +# MAVLink command: MAV_CMD_REQUEST_MESSAGE +# https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE +# Specifically used to request Information messages from Gimbal Device and Gimbal Manager +# https://mavlink.io/en/messages/common.html#GIMBAL_MANAGER_INFORMATION +# https://mavlink.io/en/messages/common.html#GIMBAL_DEVICE_INFORMATION + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv new file mode 100644 index 0000000..ef46dc9 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerCameraTrack.srv @@ -0,0 +1,28 @@ +# MAVLink commands: CAMERA_TRACK_POINT, CAMERA_TRACK_RECTANGLE, CAMERA_STOP_TRACKING +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_POINT +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_TRACK_RECTANGLE +# https://mavlink.io/en/messages/common.html#MAV_CMD_CAMERA_STOP_TRACKING + +uint8 mode # enumerator to indicate camera track mode setting - see CAMERA_TRACK_MODE +#CAMERA_TRACK_MODE +uint8 CAMERA_TRACK_MODE_POINT = 0 # If the camera supports point visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_POINT is set), this command allows to initiate the tracking. [CAMERA_TRACK_POINT] +uint8 CAMERA_TRACK_MODE_RECTANGLE = 1 # If the camera supports rectangle visual tracking (CAMERA_CAP_FLAGS_HAS_TRACKING_RECTANGLE is set), this command allows to initiate the tracking. [CAMERA_TRACK_RECTANGLE] +uint8 CAMERA_TRACK_MODE_STOP_TRACKING = 2 # Stops ongoing tracking. [CAMERA_STOP_TRACKING] + +#For CAMERA_TRACK_POINT +float32 x # Point to track x value (normalized 0..1, 0 is left, 1 is right). +float32 y # Point to track y value (normalized 0..1, 0 is top, 1 is bottom). +float32 radius # Point radius (normalized 0..1, 0 is image left, 1 is image right). + +#For CAMERA_TRACK_RECTANGLE +float32 top_left_x # Top left corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 top_left_y # Top left corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). +float32 bottom_right_x # Bottom right corner of rectangle x value (normalized 0..1, 0 is left, 1 is right). +float32 bottom_right_y # Bottom right corner of rectangle y value (normalized 0..1, 0 is top, 1 is bottom). + +#CAMERA_STOP_TRACKING doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv new file mode 100644 index 0000000..c0a2176 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerConfigure.srv @@ -0,0 +1,32 @@ +# MAVLink command: DO_GIMBAL_MANAGER_CONFIGURE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE +# Note: default MAV_COMP_ID_ONBOARD_COMPUTER = 191, see MAV_COMPONENT documentation +# https://mavlink.io/en/messages/common.html#MAV_COMPONENT + +int16 sysid_primary # Sysid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_primary # Compid for primary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 sysid_secondary # Sysid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). +int16 compid_secondary # Compid for secondary control (0: no one in control, + # -1: leave unchanged, -2: set itself in control + # (for missions where the own sysid is still unknown), + # -3: remove control if currently in control). + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). + # Note: Default Mavlink gimbal device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv new file mode 100644 index 0000000..6e3c14e --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerPitchyaw.srv @@ -0,0 +1,27 @@ +# MAVLink commands: DO_GIMBAL_MANAGER_PITCHYAW +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW + + +float32 pitch # Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode). (-180 to 180 deg) +float32 yaw # Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode). (-180 to 180 deg) +float32 pitch_rate # Pitch rate (positive to pitch up). (deg/s) +float32 yaw_rate # Yaw rate (positive to yaw to the right). (deg/s) + +uint32 flags # High level gimbal manager flags to use - See GIMBAL_MANAGER_FLAGS +#GIMBAL_MANAGER_FLAGS +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 # Based on GIMBAL_DEVICE_FLAGS_RETRACT +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 # Based on GIMBAL_DEVICE_FLAGS_NEUTRAL +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 # Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 # Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 # Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv new file mode 100644 index 0000000..02904d8 --- /dev/null +++ b/src/external/mavros_msgs/srv/GimbalManagerSetRoi.srv @@ -0,0 +1,38 @@ +# MAVLink commands: DO_SET_ROI_LOCATION, DO_SET_ROI_WPNEXT_OFFSET, DO_SET_ROI_SYSID, DO_SET_ROI_NONE +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_LOCATION +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_SYSID +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_ROI_NONE + +uint8 mode # enumerator to indicate ROI mode setting - see ROI_MODE +#ROI_MODE +uint8 ROI_MODE_LOCATION = 0 # Sets the region of interest (ROI) to a location. [DO_SET_ROI_LOCATION] +uint8 ROI_MODE_WP_NEXT_OFFSET = 1 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. [DO_SET_ROI_WPNEXT_OFFSET] +uint8 ROI_MODE_SYSID = 2 # Mount tracks system with specified system ID [DO_SET_ROI_SYSID] +uint8 ROI_MODE_NONE = 3 # Cancels any previous ROI setting and returns vehicle to defaults [DO_SET_ROI_NONE] + +uint8 gimbal_device_id # Component ID of gimbal device to address + # (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device + # components. Send command multiple times for more than + # one gimbal (but not all gimbals). Default Mavlink gimbal + # device ids: 154, 171-175 + +#For ROI_MODE_LOCATION +float32 latitude +float32 longitude +float32 altitude # Meters + +#For ROI_MODE_WP_NEXT_OFFSET +float32 pitch_offset # Pitch offset from next waypoint, positive pitching up +float32 roll_offset # Roll offset from next waypoint, positive rolling to the right +float32 yaw_offset # Yaw offset from next waypoint, positive yawing to the right + +#For ROI_MODE_SYSID +uint8 sysid # System ID to track (min: 1, max: 255) + +#ROI_MODE_NONE doesn't take extra parameters + +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/LogRequestData.srv b/src/external/mavros_msgs/srv/LogRequestData.srv new file mode 100644 index 0000000..b306df6 --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestData.srv @@ -0,0 +1,11 @@ +# Request a chunk of a log +# +# :id: - log id from LogEntry message +# :offset: - offset into the log +# :count: - number of bytes to get + +uint16 id +uint32 offset +uint32 count +--- +bool success diff --git a/src/external/mavros_msgs/srv/LogRequestEnd.srv b/src/external/mavros_msgs/srv/LogRequestEnd.srv new file mode 100644 index 0000000..956e89f --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestEnd.srv @@ -0,0 +1,4 @@ +# Stop log transfer and resume normal logging + +--- +bool success diff --git a/src/external/mavros_msgs/srv/LogRequestList.srv b/src/external/mavros_msgs/srv/LogRequestList.srv new file mode 100644 index 0000000..14eb46c --- /dev/null +++ b/src/external/mavros_msgs/srv/LogRequestList.srv @@ -0,0 +1,9 @@ +# Request a list of available logs +# +# :start: - first log id (0 for first available) +# :end: - last log id (0xffff for last available) + +uint16 start +uint16 end +--- +bool success diff --git a/src/external/mavros_msgs/srv/MessageInterval.srv b/src/external/mavros_msgs/srv/MessageInterval.srv new file mode 100644 index 0000000..5db5436 --- /dev/null +++ b/src/external/mavros_msgs/srv/MessageInterval.srv @@ -0,0 +1,7 @@ +# sets message interval +# See MAV_CMD_SET_MESSAGE_INTERVAL + +uint32 message_id +float32 message_rate +--- +bool success diff --git a/src/external/mavros_msgs/srv/MountConfigure.srv b/src/external/mavros_msgs/srv/MountConfigure.srv new file mode 100644 index 0000000..2b9afaf --- /dev/null +++ b/src/external/mavros_msgs/srv/MountConfigure.srv @@ -0,0 +1,28 @@ +# MAVLink message: DO_MOUNT_CONTROL +# https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONFIGURE + +std_msgs/Header header + +uint8 mode # See enum MAV_MOUNT_MODE. +#MAV_MOUNT_MODE +uint8 MODE_RETRACT = 0 +uint8 MODE_NEUTRAL = 1 +uint8 MODE_MAVLINK_TARGETING = 2 +uint8 MODE_RC_TARGETING = 3 +uint8 MODE_GPS_POINT = 4 + +bool stabilize_roll # stabilize roll? (1 = yes, 0 = no) +bool stabilize_pitch # stabilize pitch? (1 = yes, 0 = no) +bool stabilize_yaw # stabilize yaw? (1 = yes, 0 = no) +uint8 roll_input # roll input (See enum MOUNT_INPUT) +uint8 pitch_input # pitch input (See enum MOUNT_INPUT) +uint8 yaw_input # yaw input (See enum MOUNT_INPUT) + +#MOUNT_INPUT +uint8 INPUT_ANGLE_BODY_FRAME = 0 +uint8 INPUT_ANGULAR_RATE = 1 +uint8 INPUT_ANGLE_ABSOLUTE_FRAME = 2 +--- +bool success +# raw result returned by COMMAND_ACK +uint8 result \ No newline at end of file diff --git a/src/external/mavros_msgs/srv/ParamGet.srv b/src/external/mavros_msgs/srv/ParamGet.srv new file mode 100644 index 0000000..cf092f0 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamGet.srv @@ -0,0 +1,8 @@ +# Request parameter from attached device + +# XXX DEPRECATED: use ros2 parameters api instead + +string param_id +--- +bool success +ParamValue value diff --git a/src/external/mavros_msgs/srv/ParamPull.srv b/src/external/mavros_msgs/srv/ParamPull.srv new file mode 100644 index 0000000..db11796 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamPull.srv @@ -0,0 +1,8 @@ +# Request parameters from device +# +# Returns success status and param_recived count + +bool force_pull +--- +bool success +uint32 param_received diff --git a/src/external/mavros_msgs/srv/ParamPush.srv b/src/external/mavros_msgs/srv/ParamPush.srv new file mode 100644 index 0000000..fe0d4f8 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamPush.srv @@ -0,0 +1,9 @@ +# Send current send +# +# Returns success status and param_transfered count + +# XXX DEPRECATED: not used. param plugin listen to parameter changes + +--- +bool success +uint32 param_transfered diff --git a/src/external/mavros_msgs/srv/ParamSet.srv b/src/external/mavros_msgs/srv/ParamSet.srv new file mode 100644 index 0000000..f5b64d9 --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamSet.srv @@ -0,0 +1,9 @@ +# Request set parameter value + +# XXX DEPRECATED: replaced by ParamSetV2 + +string param_id +mavros_msgs/ParamValue value +--- +bool success +mavros_msgs/ParamValue value diff --git a/src/external/mavros_msgs/srv/ParamSetV2.srv b/src/external/mavros_msgs/srv/ParamSetV2.srv new file mode 100644 index 0000000..e06b85a --- /dev/null +++ b/src/external/mavros_msgs/srv/ParamSetV2.srv @@ -0,0 +1,14 @@ +# Request set parameter value +# +# That interface allow to bypass some checks +# and send value as is to the FCU if force_set is true. +# +# Use that api only if ROS2 Parameter API is not sufficient +# for your application. + +bool force_set +string param_id +rcl_interfaces/ParameterValue value +--- +bool success +rcl_interfaces/ParameterValue value diff --git a/src/external/mavros_msgs/srv/SetMavFrame.srv b/src/external/mavros_msgs/srv/SetMavFrame.srv new file mode 100644 index 0000000..b12f76a --- /dev/null +++ b/src/external/mavros_msgs/srv/SetMavFrame.srv @@ -0,0 +1,36 @@ +# Set MAV_FRAME for setpoints + +# XXX DEPRECATED + +# [[[cog: +# import mavros_cog +# mavros_cog.idl_decl_enum('MAV_FRAME', 'FRAME_') +# ]]] +# MAV_FRAME +uint8 FRAME_GLOBAL = 0 # Global (WGS84) coordinate frame + MSL altitude. First value / x: latitude, second value / y: longitude, third value / z: positive altitude over mean sea level (MSL). +uint8 FRAME_LOCAL_NED = 1 # NED local tangent frame (x: North, y: East, z: Down) with origin fixed relative to earth. +uint8 FRAME_MISSION = 2 # NOT a coordinate frame, indicates a mission command. +uint8 FRAME_GLOBAL_RELATIVE_ALT = 3 # Global (WGS84) coordinate frame + altitude relative to the home position. First value / x: latitude, second value / y: longitude, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 FRAME_LOCAL_ENU = 4 # ENU local tangent frame (x: East, y: North, z: Up) with origin fixed relative to earth. +uint8 FRAME_GLOBAL_INT = 5 # Global (WGS84) coordinate frame (scaled) + MSL altitude. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude over mean sea level (MSL). +uint8 FRAME_GLOBAL_RELATIVE_ALT_INT = 6 # Global (WGS84) coordinate frame (scaled) + altitude relative to the home position. First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude with 0 being at the altitude of the home location. +uint8 FRAME_LOCAL_OFFSET_NED = 7 # NED local tangent frame (x: North, y: East, z: Down) with origin that travels with the vehicle. +uint8 FRAME_BODY_NED = 8 # Same as MAV_FRAME_LOCAL_NED when used to represent position values. Same as MAV_FRAME_BODY_FRD when used with velocity/accelaration values. +uint8 FRAME_BODY_OFFSET_NED = 9 # This is the same as MAV_FRAME_BODY_FRD. +uint8 FRAME_GLOBAL_TERRAIN_ALT = 10 # Global (WGS84) coordinate frame with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees, second value / y: longitude in degrees, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 FRAME_GLOBAL_TERRAIN_ALT_INT = 11 # Global (WGS84) coordinate frame (scaled) with AGL altitude (at the waypoint coordinate). First value / x: latitude in degrees*1E7, second value / y: longitude in degrees*1E7, third value / z: positive altitude in meters with 0 being at ground level in terrain model. +uint8 FRAME_BODY_FRD = 12 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin that travels with vehicle. The forward axis is aligned to the front of the vehicle in the horizontal plane. +uint8 FRAME_RESERVED_13 = 13 # MAV_FRAME_BODY_FLU - Body fixed frame of reference, Z-up (x: Forward, y: Left, z: Up). +uint8 FRAME_RESERVED_14 = 14 # MAV_FRAME_MOCAP_NED - Odometry local coordinate frame of data given by a motion capture system, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_15 = 15 # MAV_FRAME_MOCAP_ENU - Odometry local coordinate frame of data given by a motion capture system, Z-up (x: East, y: North, z: Up). +uint8 FRAME_RESERVED_16 = 16 # MAV_FRAME_VISION_NED - Odometry local coordinate frame of data given by a vision estimation system, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_17 = 17 # MAV_FRAME_VISION_ENU - Odometry local coordinate frame of data given by a vision estimation system, Z-up (x: East, y: North, z: Up). +uint8 FRAME_RESERVED_18 = 18 # MAV_FRAME_ESTIM_NED - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-down (x: North, y: East, z: Down). +uint8 FRAME_RESERVED_19 = 19 # MAV_FRAME_ESTIM_ENU - Odometry local coordinate frame of data given by an estimator running onboard the vehicle, Z-up (x: East, y: North, z: Up). +uint8 FRAME_LOCAL_FRD = 20 # FRD local tangent frame (x: Forward, y: Right, z: Down) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +uint8 FRAME_LOCAL_FLU = 21 # FLU local tangent frame (x: Forward, y: Left, z: Up) with origin fixed relative to earth. The forward axis is aligned to the front of the vehicle in the horizontal plane. +# [[[end]]] (checksum: c5ddb537c91e87c4efba8b24c9cde50e) + +uint8 mav_frame +--- +bool success diff --git a/src/external/mavros_msgs/srv/SetMode.srv b/src/external/mavros_msgs/srv/SetMode.srv new file mode 100644 index 0000000..2637c0e --- /dev/null +++ b/src/external/mavros_msgs/srv/SetMode.srv @@ -0,0 +1,22 @@ +# set FCU mode +# +# Known custom modes listed here: +# http://wiki.ros.org/mavros/CustomModes + +# basic modes from MAV_MODE +uint8 MAV_MODE_PREFLIGHT = 0 +uint8 MAV_MODE_STABILIZE_DISARMED = 80 +uint8 MAV_MODE_STABILIZE_ARMED = 208 +uint8 MAV_MODE_MANUAL_DISARMED = 64 +uint8 MAV_MODE_MANUAL_ARMED = 192 +uint8 MAV_MODE_GUIDED_DISARMED = 88 +uint8 MAV_MODE_GUIDED_ARMED = 216 +uint8 MAV_MODE_AUTO_DISARMED = 92 +uint8 MAV_MODE_AUTO_ARMED = 220 +uint8 MAV_MODE_TEST_DISARMED = 66 +uint8 MAV_MODE_TEST_ARMED = 194 + +uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != '' +string custom_mode # string mode representation or integer +--- +bool mode_sent # Mode known/parsed correctly and SET_MODE are sent diff --git a/src/external/mavros_msgs/srv/StreamRate.srv b/src/external/mavros_msgs/srv/StreamRate.srv new file mode 100644 index 0000000..a66ca54 --- /dev/null +++ b/src/external/mavros_msgs/srv/StreamRate.srv @@ -0,0 +1,17 @@ +# sets stream rate +# See REQUEST_DATA_STREAM message + +uint8 STREAM_ALL = 0 +uint8 STREAM_RAW_SENSORS = 1 +uint8 STREAM_EXTENDED_STATUS = 2 +uint8 STREAM_RC_CHANNELS = 3 +uint8 STREAM_RAW_CONTROLLER = 4 +uint8 STREAM_POSITION = 6 +uint8 STREAM_EXTRA1 = 10 +uint8 STREAM_EXTRA2 = 11 +uint8 STREAM_EXTRA3 = 12 + +uint8 stream_id +uint16 message_rate +bool on_off +--- diff --git a/src/external/mavros_msgs/srv/VehicleInfoGet.srv b/src/external/mavros_msgs/srv/VehicleInfoGet.srv new file mode 100644 index 0000000..9e32236 --- /dev/null +++ b/src/external/mavros_msgs/srv/VehicleInfoGet.srv @@ -0,0 +1,14 @@ +# Request the Vehicle Info +# use this to request the current target sysid / compid defined in mavros +# set get_all = True to request all available vehicles + +uint8 GET_MY_SYSID = 0 +uint8 GET_MY_COMPID = 0 + +uint8 sysid +uint8 compid +bool get_all +--- +bool success +mavros_msgs/VehicleInfo[] vehicles + diff --git a/src/external/mavros_msgs/srv/WaypointClear.srv b/src/external/mavros_msgs/srv/WaypointClear.srv new file mode 100644 index 0000000..ac3d34c --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointClear.srv @@ -0,0 +1,4 @@ +# Request clear waypoint + +--- +bool success diff --git a/src/external/mavros_msgs/srv/WaypointPull.srv b/src/external/mavros_msgs/srv/WaypointPull.srv new file mode 100644 index 0000000..2d7ac2c --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointPull.srv @@ -0,0 +1,7 @@ +# Requests waypoints from device +# +# Returns success status and received count + +--- +bool success +uint32 wp_received diff --git a/src/external/mavros_msgs/srv/WaypointPush.srv b/src/external/mavros_msgs/srv/WaypointPush.srv new file mode 100644 index 0000000..e29727b --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointPush.srv @@ -0,0 +1,11 @@ +# Send waypoints to device +# +# :start_index: will define a partial waypoint update. Set to 0 for full update +# +# Returns success status and transfered count + +uint16 start_index +mavros_msgs/Waypoint[] waypoints +--- +bool success +uint32 wp_transfered diff --git a/src/external/mavros_msgs/srv/WaypointSetCurrent.srv b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv new file mode 100644 index 0000000..b99612f --- /dev/null +++ b/src/external/mavros_msgs/srv/WaypointSetCurrent.srv @@ -0,0 +1,7 @@ +# Request set current waypoint +# +# wp_seq - index in waypoint array + +uint16 wp_seq +--- +bool success diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt new file mode 100644 index 0000000..909a6b8 --- /dev/null +++ b/src/fc_interfaces/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.8) +project(fc_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/MavPing.srv" + "srv/MavCommandLong.srv" + ) + +ament_package() diff --git a/src/fc_interfaces/package.xml b/src/fc_interfaces/package.xml new file mode 100644 index 0000000..292f81b --- /dev/null +++ b/src/fc_interfaces/package.xml @@ -0,0 +1,21 @@ + + + + fc_interfaces + 0.0.0 + TODO: Package description + picars + TODO: License declaration + + ament_cmake + rosidl_default_generators + rosidl_interface_packages + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/fc_interfaces/srv/MavCommandLong.srv b/src/fc_interfaces/srv/MavCommandLong.srv new file mode 100644 index 0000000..3ff2e6e --- /dev/null +++ b/src/fc_interfaces/srv/MavCommandLong.srv @@ -0,0 +1,18 @@ +# Request +uint8 target_sysid +uint8 target_compid +uint16 command +uint8 confirmation +float32 param1 +float32 param2 +float32 param3 +float32 param4 +float32 param5 +float32 param6 +float32 param7 +float32 timeout_sec +--- +# Response +bool success +string message +uint8 ack_result \ No newline at end of file diff --git a/src/fc_interfaces/srv/MavPing.srv b/src/fc_interfaces/srv/MavPing.srv new file mode 100644 index 0000000..400baf0 --- /dev/null +++ b/src/fc_interfaces/srv/MavPing.srv @@ -0,0 +1,9 @@ +# Request +uint8 target_sysid +uint8 target_compid +uint8 ping_seq +--- +# Response +bool success +string message +float32 rtt_ms \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index ae89d72..6625de1 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -VERSION_NO = "v0.59" +VERSION_NO = "v0.60" class PanelState: def __init__(self): @@ -1138,8 +1138,7 @@ class Orchestrator: # === 4) ros 部分的準備 === self.fc_ros_manager = mros.ros2_manager - if not self.fc_ros_manager.initialized: - self.fc_ros_manager.initialize() + self.fc_ros_manager.initialize() self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = { 'position': 1.0, 'attitude': 0.0, @@ -1311,7 +1310,7 @@ class Orchestrator: if self.fc_ros_manager.spin_thread.is_alive(): if self.fc_ros_manager.running: - self.fc_ros_manager.stop() + self.fc_ros_manager.shutdown() self.fc_ros_manager.spin_thread.join(timeout=2) # 關閉面板執行緒 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py b/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py deleted file mode 100644 index 47a1c92..0000000 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py +++ /dev/null @@ -1,96 +0,0 @@ - -# 自定義的 import -from .theLogger import setup_logger - -# ====================== 分割線 ===================== - -logger = setup_logger("mavlinkDevice.py") - -# 用來記錄每個 system 的資訊 -# 資料格式 { sysid : mavlink_device object } -mavlink_systems = {} - -# ====================== 分割線 ===================== - -# 這個 class 是用來記錄每個 system 的資訊 每個 system 可能會有多個 component -class mavlink_device(): - def __init__(self): - self.socket_id = None # 記錄來自於哪個 socket - self.sysid = None - self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object - - def __str__(self): - p_str = '=====================\n' - p_str += f'object id : {id(self)}\n' # debug - p_str += f'socket_id : {self.socket_id}\n' - p_str += f'sysid : {self.sysid}\n' - p_str += 'has components : \n' - for compid in self.components: - p_str += f'compid : {compid}\n' - p_str += f'mav_type : {self.components[compid].mav_type}\n' - # p_str += '=====================\n' - return p_str - - ''' - 寫了半天 這個功能應該是 pymalink 本來就有的 - 去找 pymavlink_util.py 的 mavfile(object) - 算了 先擺著吧 之後再看看怎麼整合 - ''' - def updateComponentPacketCount(self, compid, current_seq, current_type, current_time): - # 這段檢查遺失封包 - try: - last_seq = self.components[compid].msg_seq - except KeyError: - # 這個 component id 還不存在 - # logger.error('System ID : {} This Component ID : {} Did not init yet'.format(self.sysid, compid)) # 因為初始化的之前 會有大量非 heartbeat 的訊息進來 這是正常現象 TODO 之後要幫這個類別加上初始化狀態 再進行這個判斷 - return - - if last_seq != None: - expected_seq = (last_seq + 1) % 256 - diff = current_seq - expected_seq - # print("current last exp diff : ",current_seq, last_seq, expected_seq, diff) # debug - if diff < 0: - diff += 256 - self.components[compid].lost_packet_count += diff - - # 這段更新封包的基本資訊 - self.components[compid].msg_seq = current_seq - self.components[compid].last_msg_time = current_time - if current_type in self.components[compid].msg_count: - self.components[compid].msg_count[current_type] += 1 - else: - self.components[compid].msg_count[current_type] = 1 - - def resetComponentPacketCount(self, compid): - self.components[compid].msg_count = {} - self.components[compid].msg_seq = None - self.components[compid].lost_packet_count = 0 - - class mavlink_component(): - # 程式用不到的參數 但是做個記錄 - # paraEmitList = ['base_mode', 'flightMode_mode', - # 'battery_voltage', # from BATTERY_STATUS (147) - # 'gps_fix_type', # from GPS_RAW_INT (24) - # 'roll', 'pitch', 'yaw', # from ATTITUDE (30) - # 'position_latitude', 'position_longitude', 'position_altitude', # from GLOBAL_POSITION_INT (33) - # 'heading', # for VFR_HUD (74) - # ] - - def __init__(self): - self.mav_type = None # 表示 Vehicle 或 component type (例如: 旋翼機是2, 雲台是26, GCS是6) - self.mav_autopilot = None # 表示 autopilot type (例如: ardupilot是3, px4是12) - self.mav_system_status = None # 表示 system status (例如: active是3, standby是4) - - # 紀錄從這個 component 收到最後的訊息序號和時間 - self.msg_count = {} - self.msg_seq = None - self.lost_packet_count = 0 - self.last_msg_time = 0 - - # 存放該 emit component 參數的區域 - # 內容格式為 {param_name(字串) : param_value} - # param_name 請見上面 paraEmitList - self.emitParams = {} - # 用來存放每個 topic 的 publisher - # 內容格式 為 {topic_name(字串) : [publisher(物件), method(函式)]} (? - self.publishers = {} \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index c8cfaed..694913c 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -45,6 +45,8 @@ from collections import deque # mavlink 的 import from pymavlink import mavutil +from pymavlink.dialects.v20 import common as mav_common +from pymavlink.dialects.v20 import ardupilotmega as mav_ardupilot # 自定義的 import from .mavlinkVehicleView import ( @@ -111,7 +113,7 @@ class mavlink_bridge: 30: self._handle_attitude, # ATTITUDE 32: self._handle_local_position, # LOCAL_POSITION_NED 33: self._handle_global_position, # GLOBAL_POSITION_INT - 74: self._handle_vfr_hud, # VFR_HUD + 74: self._handle_vfr_hud, # VFR_HUD 147: self._handle_battery_status, # BATTERY_STATUS } @@ -342,6 +344,19 @@ class mavlink_object: mavlinkObjects = {} # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) } socket_num = 0 # 用來記錄目前的 socket 數量 + # 用來銜接 pymavlink 的 MavLink 實例的管道 + class CapturingPipeline: + def __init__(self, target_deque): + self.target_deque = target_deque + self.last_data = b'' + def write(self, data): + self.last_data = bytes(data) + # logger.debug(f'CapturingPipeline write data: {data.hex()}') # developer debug + # 把收到的資料 bytes 放到 target_deque + self.target_deque.append(data) + def seek(self, pos): pass + def tell(self): return 0 + def __new__(cls, *args, **kwargs): # 每創建一個實例 就將其添加到 mavlinkObjects 列表中 # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 @@ -359,16 +374,20 @@ class mavlink_object: cls.mavlinkObjects[socket_id] = instance return instance - def __init__(self, socket): + def __init__(self, socket, socket_dialect = 'common'): # 登入所需的 socket self.mavlink_socket = socket # 用於主線程發送消息的緩衝區 self.outgoing_msgs = deque() + self.mavlinkPipeline = self.CapturingPipeline(self.outgoing_msgs) + + if socket_dialect == 'common': + self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191) # 記錄訊息過濾類型 (可選) self.bridge_msg_types = set([0, 30, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS - self.return_msg_types = set() + self.return_msg_types = set([]) # 轉發到別的 mavlink object 作為目標端口 的列表 self.target_sockets = set() diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py b/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py deleted file mode 100644 index 5dc3ca1..0000000 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py +++ /dev/null @@ -1,212 +0,0 @@ - -''' -這個檔案只有一個 class -是作為 mavlinkObject.py 中 mavlink_analyzer 類別的功能衍生 - -主要概念是將 "離散的" mavlink 參數轉換成 ROS topic -包含了創建 publisher 和 以及包裝並丟到 ros2 topic 的 packEmit - -publisher topic name 命名規則為 <前綴詞>/s/<具體 topic> -''' - -import os - -# ROS2 的 import -import std_msgs.msg -import sensor_msgs.msg -import geometry_msgs.msg -import mavros_msgs.msg - -import math - -# 自定義的 import -from .utils import setup_logger - -logger = setup_logger(os.path.basename(__file__)) - -class mavlink_publisher(): - - prefix_path = 'MavLinkBus' - - def create_flightMode(self, sysid, component_obj): - # target topic name # 請跟這個 method 的名稱保持一致 - target_topic = 'flightMode' - - # 這邊要檢查 flight_mode 是否存在 - try: - _ = component_obj.emitParams['flightMode_mode'] - except KeyError: - # 這個 component id 還不存在 - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - # 若存在則 建立 publisher object 並回傳 true - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode] - - return True - - def packEmit_flightMode(cls, emitParams, publisher): - msg_str = emitParams['flightMode_mode'] - msg = std_msgs.msg.String() - msg.data = msg_str - publisher.publish(msg) - pass - - # ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同 ros2 topic 訊息 請放在這裡 ↓↓↓↓↓↓↓↓↓↓↓↓ - def euler_to_quaternion(cls,roll, pitch, yaw): - qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) - qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - return [qx, qy, qz, qw] - - def create_attitude(self, sysid, component_obj): - target_topic = 'attitude' - - try: - _ = component_obj.emitParams['attitude'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.Imu, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_attitude] - - def packEmit_attitude(self, emitParams, publisher): - mav_msg = emitParams['attitude'] - msg = sensor_msgs.msg.Imu() - x, y, z, w = self.euler_to_quaternion(mav_msg.roll, mav_msg.pitch, mav_msg.yaw) - msg.orientation.x = x - msg.orientation.y = y - msg.orientation.z = z - msg.orientation.w = w - msg.angular_velocity.x = mav_msg.rollspeed - msg.angular_velocity.y = mav_msg.pitchspeed - msg.angular_velocity.z = mav_msg.yawspeed - publisher.publish(msg) - pass - - def create_local_position_pose(self, sysid, component_obj): - target_topic = 'local_position/pose' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(geometry_msgs.msg.Point, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_pose] - - def packEmit_local_pose(cls, emitParams, publisher): - mav_msg = emitParams['local_position'] - msg = geometry_msgs.msg.Point() - msg.x = mav_msg.x - msg.y = mav_msg.y - msg.z = mav_msg.z - publisher.publish(msg) - pass - - def create_local_position_velocity(self, sysid, component_obj): - target_topic = 'local_position/velocity' - try: - _ = component_obj.emitParams['local_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(geometry_msgs.msg.Vector3, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_local_vel] - - def packEmit_local_vel(cls, emitParams, publisher): - mav_msg = emitParams['local_position'] - msg = geometry_msgs.msg.Vector3() - msg.x = mav_msg.vx - msg.y = mav_msg.vy - msg.z = mav_msg.vz - publisher.publish(msg) - pass - - def create_global_global(self, sysid, component_obj): - target_topic = 'global_position/global' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.NavSatFix, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_global] - - def packEmit_global_global(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = sensor_msgs.msg.NavSatFix() - msg.latitude = mav_msg.lat/1e7 - msg.longitude = mav_msg.lon/1e7 - msg.altitude = mav_msg.alt/1e3 - publisher.publish(msg) - pass - - def create_global_rel(self, sysid, component_obj): - target_topic = 'global_position/rel_alt' - try: - _ = component_obj.emitParams['global_position'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(std_msgs.msg.Float64, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_global_rel] - - def packEmit_global_rel(cls, emitParams, publisher): - mav_msg = emitParams['global_position'] - msg = std_msgs.msg.Float64() - msg.data = float(mav_msg.relative_alt/1e3) - publisher.publish(msg) - pass - - def create_vfr_hud(self, sysid, component_obj): - target_topic = 'vfr_hud' - try: - _ = component_obj.emitParams['vfr_hud'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(mavros_msgs.msg.VfrHud, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_vfr_hud] - - def packEmit_vfr_hud(cls, emitParams, publisher): - mav_msg = emitParams['vfr_hud'] - msg = mavros_msgs.msg.VfrHud() - msg.airspeed = mav_msg.airspeed - msg.groundspeed = mav_msg.groundspeed - msg.heading = mav_msg.heading - msg.throttle = float(mav_msg.throttle) - msg.altitude = mav_msg.alt - msg.climb = mav_msg.climb - publisher.publish(msg) - pass - - def create_battery(self, sysid, component_obj): - target_topic = 'battery' - try: - _ = component_obj.emitParams['battery'] - except KeyError: - logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid'])) - return False - topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic) - publisher_ = self.create_publisher(sensor_msgs.msg.BatteryState, topic_name, 1) - component_obj.publishers[target_topic] = [publisher_, self.packEmit_battery] - - def packEmit_battery(cls, emitParams, publisher): - mav_msg = emitParams['battery'] - msg = sensor_msgs.msg.BatteryState() - msg.voltage = mav_msg.voltages[0]/1e3 - publisher.publish(msg) - pass - - - # ↑↑↑↑↑↑↑↑↑↑↑↑ 處理不同 ros2 topic 訊息 請放在這裡 ↑↑↑↑↑↑↑↑↑↑↑↑ \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index a95fb26..cebcc1b 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -24,8 +24,12 @@ import sensor_msgs.msg import geometry_msgs.msg import mavros_msgs.msg +# ROS2 Service imports +import fc_interfaces.srv as fcsrv + # 自定義 imports from . import mavlinkVehicleView as mvv +from . import mavlinkObject as mo from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) @@ -450,195 +454,263 @@ class VehicleStatusPublisher(Node): # MavlinkCommandService Node # ============================================================================ +class PendingEntry(): + def __init__(self, event, match_fn): + # self.match_msgid = match_msgid # 只會在初始時設定 + self.match_fn = match_fn + + self.event = event # 只會在外迴圈 set() + self.result_mav_msg = None #只會在外迴圈 被寫入 + self.error = "" + class MavlinkCommandService(Node): """ MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令 職責: - - 作為 service server，等待 client 請求 + - 作為 service server 等待 client 請求 - 接收請求，組裝 MAVLink 封包 - 調用 mavlinkObject 發送封包 - 處理 ACK 等待和超時（未來實現） 設計理念：回歸 MAVLink 純粹結構 - 只負責將 ROS2 請求轉換為 MAVLink 封包 - - 不預設功能（如 ARM/DISARM），保持模組化 + - 不預設功能（如 ARM/DISARM) 保持模組化 - 高層應用可透過此 service 實現各種功能 """ + + serviceString_prefix = '/fc_network/vehicle' def __init__(self): super().__init__('mavlink_command_service') - # ═══════════════════════════════════════════════════════════════════ - # ROS2 Service 架構說明： - # - # 1. Service 定義：由 .srv 檔案定義（Request + Response） - # - Request: client 發送的請求內容 - # - Response: server 回傳的結果 - # - # 2. Service Server 創建： - # self.create_service(srv_type, service_name, callback_function) - # - srv_type: service 的訊息類型（需要自定義或使用標準） - # - service_name: service 的名稱（client 用此名稱呼叫） - # - callback_function: 處理請求的回調函數 - # - # 3. Callback 函數： - # def callback(self, request, response): - # # request: 包含 client 發送的數據 - # # response: 需要填充並返回給 client - # return response - # - # 4. Service Client 使用方式（在其他程式中）： - # client = node.create_client(srv_type, service_name) - # request = srv_type.Request() - # future = client.call_async(request) # 異步調用 - # # 或 response = client.call(request) # 同步調用 - # ═══════════════════════════════════════════════════════════════════ - - # 由於 ROS2 自定義 service 需要 .srv 檔案編譯 - # 這裡先使用標準 String service 作為簡化實現 - # TODO: 未來可創建專門的 .srv 檔案 - from std_srvs.srv import Trigger - from example_interfaces.srv import SetBool + # 狀態標記 + self.running = True + # mavlinkObject 的引用（將由外部設置） + self.mavlink_analyzer = None + + # pending 旗標物件的儲存庫 + self._pending_by_sysid = {} # sysid(int) : PendingEntry + # ═══════════════════════════════════════════════════════════════════ - # Service 1: 發送 MAVLink Message（通用介面） - # 使用 Trigger 作為臨時實現，未來應使用自定義 service + # Service : ADD_TWO 測試用，未來刪除 # ═══════════════════════════════════════════════════════════════════ - # TODO: 創建 SendMavlinkMessage.srv - # Request: - # uint8 target_sysid - # uint8 target_compid - # uint16 message_id - # string fields_json # JSON 格式的字段數據 - # bool wait_response - # uint16 response_msgid - # float32 timeout - # Response: - # bool success - # string response_json - # string error_message - - # 暫時使用簡化版本（僅示範架構） - self.srv_send_message = self.create_service( - Trigger, - '/mavlink/send_message', - self.handle_send_message + from example_interfaces.srv import AddTwoInts + self.srv_test = self.create_service( + AddTwoInts, + 'mavlink/add_two_ints', + self.handle_add_two_ints ) - + # ═══════════════════════════════════════════════════════════════════ - # Service 2: 發送 COMMAND_LONG + # Service : TIMESYNC 可以作為模板 # ═══════════════════════════════════════════════════════════════════ - self.srv_command_long = self.create_service( - Trigger, - '/mavlink/send_command_long', - self.handle_command_long + self.srv_mav_ping = self.create_service( + fcsrv.MavPing, + self.serviceString_prefix + '/mav_ping', + self.handle_mav_timesync_ping ) - + # ═══════════════════════════════════════════════════════════════════ - # Service 3: 參數請求 + # Service : 發送 COMMAND_LONG # ═══════════════════════════════════════════════════════════════════ - self.srv_param_request = self.create_service( - Trigger, - '/mavlink/param_request', - self.handle_param_request + self.srv_command_long = self.create_service( + fcsrv.MavCommandLong, + self.serviceString_prefix + '/send_command_long', + self.handle_command_long ) - - # 狀態標記 - self.running = True - - # mavlinkObject 的引用（將由外部設置） - self.mavlink_analyzer = None - + logger.info("MavlinkCommandService initialized") - - def set_mavlink_analyzer(self, mavlink_analyzer): - """ - 設置 mavlink_analyzer 引用 - - Args: - mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例 - """ - self.mavlink_analyzer = mavlink_analyzer - logger.info("MavlinkCommandService: mavlink_analyzer set") - + + def _index(self, target_sysid, target_compid): + + # 找到對應的 vehicle + vehicle = mvv.vehicle_registry.get(target_sysid) + if not vehicle: + return None + + socket_id = vehicle.custom_meta.get("socket_id") + if socket_id is None: + return None + + # 提取主要的 socket_id + mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id) + if mav_obj is None: + return None + return mav_obj + + def return_router(self): + ''' + 這邊是給外部迴圈呼叫的 消耗 return_packet_ring 裡接收到的 mavlink 封包 + 分送到各自的 pending 中 + 藉由 event.set() 解開 service 中的 block + ''' + return_tuple = mo.return_packet_ring.get() + # 確認 return_packet_ring 有資料 + if return_tuple == None: return + + socketid, timestamp, msg = return_tuple + sysid = msg.get_srcSystem() + + _pending = self._pending_by_sysid.get(sysid) + # 確認是否有 service 在等待回應 若無直接 return 此封包也會被忽略 + if _pending == None: + return + + if _pending.match_fn(msg): + _pending.result_mav_msg = msg + _pending.event.set() + # ═══════════════════════════════════════════════════════════════════════ - # Service Handler: 發送 MAVLink Message + # Service Handler: 發送 TIMESYNC 可以作為模板 # ═══════════════════════════════════════════════════════════════════════ - def handle_send_message(self, request, response): - """ - 處理發送 MAVLink 訊息的請求 - - ROS2 Service Callback 說明： - - 此函數會在 client 調用 service 時被執行 - - request: 包含 client 傳入的參數 - - response: 需要填充結果並返回給 client - - 必須 return response - - Args: - request: Trigger.Request (暫時使用，未來改為自定義) - response: Trigger.Response - - Returns: - response: 填充後的回應 - """ - logger.info("Received send_message request") - - # 檢查 mavlink_analyzer 是否已設置 - if self.mavlink_analyzer is None: - response.success = False - response.message = "Error: mavlink_analyzer not set" - logger.error(response.message) + def handle_mav_timesync_ping(self, request, response): + ''' + 用 timesync 封包驗證來回的時間 + ''' + # 設定失效回應 + response.success = False + response.message = "Unknown error" + response.rtt_ms = 0.0 + timeout_sec = 2.0 + + # 1) 確認是否已經有相同 sysid 的其他需求正在 pending + if request.target_sysid in self._pending_by_sysid: + response.message = f"sysid {request.target_sysid} already has pending request" return response - - # TODO: 實際實現 - # 1. 從 request 解析參數（target_sysid, message_id, fields 等） - # 2. 使用 pymavlink 組裝 MAVLink 封包 - # 3. 調用 mavlink_analyzer.send_message() 發送 - # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應 - - # 暫時返回成功（示範用） - response.success = True - response.message = "Message sent (placeholder implementation)" + + # 2) 找到 socket 標地 + socketObject = self._index(request.target_sysid, request.target_compid) + if socketObject is None: + response.message = "This system id not found." + return response + + # 3) 接收封包系統 的設定 + # 在 socket 那邊先把要的封包種類導流進來 + expect_recieve_msg_id = 111 # TIMESYNC + socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id]) + evt = threading.Event() + # 設定封包檢驗 + # 這邊是設定 tc1 不為 0 + def match_fn(compare_msg): + if compare_msg.get_msgId() != expect_recieve_msg_id : + return False + # logger.debug(f"mark A : {compare_msg.get_srcSystem()} ,{compare_msg.get_msgId()}, {compare_msg.tc1}, {compare_msg.ts1}") + if compare_msg.tc1 == 0 : + return False + return True + + _pending = PendingEntry(event = evt, match_fn = match_fn) + + self._pending_by_sysid[request.target_sysid] = _pending + + # 4) 送出封包 + now_us = int(time.monotonic() * 1e6) + socketObject.MAVLink.timesync_send(0, now_us) + + fail_skip = False + # 5) 等待回應封包 + if not evt.wait(timeout_sec): + response.message = "waiting Timeout - TIMESYNC" + fail_skip = True + + # 6) 處理回應封包 + if not fail_skip: + ack_msg = _pending.result_mav_msg + + current_time = int(time.monotonic() * 1e6) + response.rtt_ms = (current_time - now_us) / 1e3 + response.message = "" + + # 7) 接收封包系統 的重置 + msg_types = list(socketObject.return_msg_types) + if expect_recieve_msg_id in msg_types: + msg_types.remove(expect_recieve_msg_id) + socketObject.set_return_message_types(msg_types) + del evt + self._pending_by_sysid.pop(request.target_sysid, None) + return response - + + def handle_add_two_ints(self, request, response): + """測試用 Service Handler 未來刪除""" + logger.info(f"Received add_two_ints request: {request.a} + {request.b}") + response.sum = request.a + request.b + logger.info( + f"[add_two_ints] thread_ident={threading.get_ident()} time={time.time()}" + ) + time.sleep(1) + + return response + # ═══════════════════════════════════════════════════════════════════════ # Service Handler: 發送 COMMAND_LONG # ═══════════════════════════════════════════════════════════════════════ def handle_command_long(self, request, response): - """ - 處理發送 COMMAND_LONG 的請求 - - COMMAND_LONG (MAVLink message ID=76): - - 用於發送簡單命令給載具 - - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等 - - Args: - request: Trigger.Request - response: Trigger.Response - - Returns: - response: 填充後的回應 - """ - logger.info("Received command_long request") - - if self.mavlink_analyzer is None: - response.success = False - response.message = "Error: mavlink_analyzer not set" + # 設定失效回應 + response.success = False + response.message = "Unknown error" + timeout_sec = request.timeout_sec + + # 1) 確認是否已經有相同 sysid 的其他需求正在 pending + if request.target_sysid in self._pending_by_sysid: + response.message = f"sysid {request.target_sysid} already has pending request" return response - - # TODO: 實際實現 - # 1. 從 request 解析 COMMAND_LONG 參數 - # - target_sysid, target_compid - # - command (MAV_CMD_xxx) - # - param1~param7 - # 2. 組裝 COMMAND_LONG 封包 - # 3. 發送並等待 COMMAND_ACK (message ID=77) - # 4. 解析 ACK 結果（ACCEPTED/FAILED 等） - - response.success = True - response.message = "Command sent (placeholder implementation)" + + # 2) 找到 socket 標地 + socketObject = self._index(request.target_sysid, request.target_compid) + if socketObject is None: + response.message = "This system id not found." + return response + + # 3) 接收封包系統 的設定 + expect_recieve_msg_id = 77 # mavutil.mavlink.MAVLINK_MSG_ID_COMMAND_ACK + socketObject.set_return_message_types(list(socketObject.return_msg_types) + [expect_recieve_msg_id]) + evt = threading.Event() + + def match_fn(compare_msg): + if compare_msg.get_msgId() != expect_recieve_msg_id : + return False + return True + + _pending = PendingEntry(event = evt, match_fn = match_fn) + + self._pending_by_sysid[request.target_sysid] = _pending + + # 4) 送出封包 + socketObject.MAVLink.command_long_send( + request.target_sysid, + request.target_compid, + request.command, + request.confirmation, + request.param1, request.param2, request.param3, request.param4, + request.param5, request.param6, request.param7 + ) + + fail_skip = False + # 5) 等待回應封包 + if not evt.wait(timeout_sec): + response.message = "waiting Timeout - CommLONG" + fail_skip = True + + # 6) 處理回應封包 + if not fail_skip: + ack_msg = _pending.result_mav_msg + response.success = (ack_msg.result == 0) # mavutil.mavlink.MAV_RESULT_ACCEPTED + response.message = "" # 沒有消息就是好消息 + response.ack_result = ack_msg.result + + # 7) 接收封包系統 的重置 + msg_types = list(socketObject.return_msg_types) + if expect_recieve_msg_id in msg_types: + msg_types.remove(expect_recieve_msg_id) + socketObject.set_return_message_types(msg_types) + del evt + self._pending_by_sysid.pop(request.target_sysid, None) + return response + # ═══════════════════════════════════════════════════════════════════════ # Service Handler: 參數請求 @@ -719,12 +791,6 @@ class MavlinkCommandService(Node): response.message = "Param request sent (placeholder implementation)" return response - # ═══════════════════════════════════════════════════════════════════════ - # 【新增 Service 位置 4/4】 - # 若要新增 service，請在此處添加新的 handler 方法 - # 並在 __init__ 中創建對應的 service server - # ═══════════════════════════════════════════════════════════════════════ - def stop(self): """停止服務""" self.running = False @@ -739,6 +805,11 @@ class fc_ros_manager: """ MAVLink ROS2 節點管理器 + 因為物件創建在 mavlinkROS2Nodes.py 中，所以會分為 __init__ 跟 initialize 兩個階段。 + + stop 是停下止 ROS2 nodes 的運行，但不銷毀節點實例，允許後續再次 start。 + shutdown 是完全關閉 ROS2 並銷毀節點實例。 + 管理兩個獨立的 ROS2 Node： - VehicleStatusPublisher - MavlinkCommandService @@ -750,7 +821,7 @@ class fc_ros_manager: self.initialized = False self.running = False - # 两个 node 实例 + # 两个 node 實例 self.status_publisher: Optional[VehicleStatusPublisher] = None self.command_service: Optional[MavlinkCommandService] = None @@ -772,7 +843,7 @@ class fc_ros_manager: self.status_publisher = VehicleStatusPublisher() self.command_service = MavlinkCommandService() - # 創建執行者 MultiThreadedExecutor + # 創建執行者 MultiThreadedExecutor 並把 node 加入其中 self.executor = MultiThreadedExecutor() self.executor.add_node(self.status_publisher) self.executor.add_node(self.command_service) @@ -813,12 +884,14 @@ class fc_ros_manager: self.running = False return False + # 循環執行的地方 def _spin_executor(self): """在 thread 中運行的 executor""" try: # logger.info("ROS2 executor spinning...") while self.running: self.executor.spin_once(timeout_sec=0.1) + self.command_service.return_router() except Exception as e: logger.error(f"fc_ros_manager error in spinning executor: {e}") @@ -899,4 +972,8 @@ ros2_manager = fc_ros_manager() 4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布 5. 預留 MavlinkCommandService 結構（稍後實現） +2026.03.27 +1. 完成 ros2 service 結構與 timesync command_long 協議 + ''' + diff --git a/src/fc_network_adapter/fc_network_adapter/ringBuffer.py b/src/fc_network_adapter/fc_network_adapter/ringBuffer.py deleted file mode 100644 index 6728426..0000000 --- a/src/fc_network_adapter/fc_network_adapter/ringBuffer.py +++ /dev/null @@ -1,231 +0,0 @@ -# import array -import threading -import ctypes -from typing import Any, Optional, Tuple - -class RingBuffer: - """ - 高效能無鎖環形緩衝區，使用原子操作避免鎖定 - 用於在不同執行緒間高效傳遞資料 - """ - # 緩衝區計數器，用於自動分配 buffer_id - _buffer_counter = 0 - _counter_lock = threading.Lock() - - def __init__(self, capacity: int = 256, buffer_id: int = None): - """ - 初始化環形緩衝區 - - Args: - capacity: 緩衝區容量 (必須是 2 的次方) - buffer_id: 緩衝區 ID，如果為 None 則自動分配 - """ - # 確保容量是 2 的次方，便於使用位運算取模 - if capacity & (capacity - 1) != 0: - # 找到大於等於 capacity 的最小 2 的次方 - capacity = 1 << (capacity - 1).bit_length() - - # 分配緩衝區 ID - if buffer_id is None: - with RingBuffer._counter_lock: - buffer_id = RingBuffer._buffer_counter - RingBuffer._buffer_counter += 1 - - self.buffer_id = buffer_id - self.capacity = capacity - self.mask = capacity - 1 # 用於快速取模 - self.buffer = [None] * capacity - - # 原子計數器作為讀寫指標 - self.write_index = ctypes.c_uint64(0) - self.read_index = ctypes.c_uint64(0) - - # 用於檢測上次操作的執行緒 ID - self._last_read_thread = None - self._last_write_thread = None - - # 添加同時讀寫統計 - self.concurrent_write_count = ctypes.c_uint64(0) # 同時寫入計數 - self.concurrent_read_count = ctypes.c_uint64(0) # 同時讀取計數 - self.total_write_count = ctypes.c_uint64(0) # 總寫入操作計數 - self.total_read_count = ctypes.c_uint64(0) # 總讀取操作計數 - self.overflow_count = ctypes.c_uint64(0) # 緩衝區溢出次數 - - # 記錄各執行緒的操作次數 - self.thread_write_counts = {} # {thread_id: count} - self.thread_read_counts = {} # {thread_id: count} - - # 用於保護統計數據的鎖（僅用於統計，不影響主要讀寫操作） - self._stats_lock = threading.Lock() - - def put(self, item: Any) -> bool: - """ - 將項目存入緩衝區 - - Args: - item: 要存入的項目 - - Returns: - bool: 成功寫入返回 True，緩衝區已滿返回 False - """ - # 更新寫入統計 - self.total_write_count.value += 1 - - # 檢測上次寫入的執行緒 - current_thread = threading.get_ident() - - # 記錄當前執行緒寫入次數 - with self._stats_lock: - self.thread_write_counts[current_thread] = self.thread_write_counts.get(current_thread, 0) + 1 - - # 檢測是否為不同執行緒同時寫入 - if self._last_write_thread is not None and current_thread != self._last_write_thread: - self.concurrent_write_count.value += 1 - - self._last_write_thread = current_thread - - # 原子獲取當前寫入位置 - current = self.write_index.value - next_pos = (current + 1) & self.mask - - # 檢查緩衝區是否已滿 - if next_pos == self.read_index.value: - self.overflow_count.value += 1 - return False - - # 寫入資料並原子更新寫入位置 - self.buffer[current] = item - self.write_index.value = next_pos - return True - - def get(self) -> Optional[Any]: - """ - 從緩衝區讀取項目 - - Returns: - 項目或 None（如果緩衝區為空） - """ - # 更新讀取統計 - self.total_read_count.value += 1 - - # 檢測上次讀取的執行緒 - current_thread = threading.get_ident() - - # 記錄當前執行緒讀取次數 - with self._stats_lock: - self.thread_read_counts[current_thread] = self.thread_read_counts.get(current_thread, 0) + 1 - - # 檢測是否為不同執行緒同時讀取 - if self._last_read_thread is not None and current_thread != self._last_read_thread: - self.concurrent_read_count.value += 1 - - self._last_read_thread = current_thread - - # 檢查緩衝區是否為空 - if self.read_index.value == self.write_index.value: - return None - - # 原子獲取當前讀取位置並讀取資料 - current = self.read_index.value - item = self.buffer[current] - - # 原子更新讀取位置 - self.read_index.value = (current + 1) & self.mask - return item - - def get_all(self) -> list: - """ - 獲取緩衝區中的所有項目 - - Returns: - list: 所有可用項目的列表 - """ - items = [] - while True: - item = self.get() - if item is None: - break - items.append(item) - return items - - def size(self) -> int: - # 返回目前緩衝區內項目數量 - return (self.write_index.value - self.read_index.value) & self.mask - - def is_empty(self) -> bool: - # 檢查緩衝區是否為空 - return self.read_index.value == self.write_index.value - - def is_full(self) -> bool: - # 檢查緩衝區是否已滿 - return ((self.write_index.value + 1) & self.mask) == self.read_index.value - - def clear(self) -> None: - """清空緩衝區""" - self.read_index.value = self.write_index.value - - def get_stats(self) -> dict: - """ - 獲取緩衝區統計資訊 - - Returns: - dict: 包含各種統計數據的字典 - """ - with self._stats_lock: - stats = { - "buffer_id": self.buffer_id, - "capacity": self.capacity, - "current_size": self.size(), - "is_full": self.is_full(), - "is_empty": self.is_empty(), - "total_writes": self.total_write_count.value, - "total_reads": self.total_read_count.value, - "concurrent_writes": self.concurrent_write_count.value, - "concurrent_reads": self.concurrent_read_count.value, - "overflow_count": self.overflow_count.value, - "write_threads": len(self.thread_write_counts), - "read_threads": len(self.thread_read_counts), - "concurrent_write_ratio": self.concurrent_write_count.value / max(1, self.total_write_count.value), - "concurrent_read_ratio": self.concurrent_read_count.value / max(1, self.total_read_count.value), - "top_writer_threads": sorted(self.thread_write_counts.items(), key=lambda x: x[1], reverse=True)[:3], - "top_reader_threads": sorted(self.thread_read_counts.items(), key=lambda x: x[1], reverse=True)[:3], - } - return stats - - def print_stats(self) -> None: - """輸出當前緩衝區統計資訊""" - stats = self.get_stats() - - print(f"\n=== RingBuffer #{stats['buffer_id']} Statistics ===") - print(f"Capacity: {stats['capacity']}, Current Size: {stats['current_size']}") - print(f"Total Write Operations: {stats['total_writes']}") - print(f"Total Read Operations: {stats['total_reads']}") - print(f"Concurrent Write Events: {stats['concurrent_writes']} ({stats['concurrent_write_ratio']:.2%})") - print(f"Concurrent Read Events: {stats['concurrent_reads']} ({stats['concurrent_read_ratio']:.2%})") - print(f"Buffer Overflow Count: {stats['overflow_count']}") - print(f"Writing Threads: {stats['write_threads']}") - print(f"Reading Threads: {stats['read_threads']}") - - print("Top Writer Threads:") - for thread_id, count in stats['top_writer_threads']: - print(f" Thread {thread_id}: {count} writes") - - print("Top Reader Threads:") - for thread_id, count in stats['top_reader_threads']: - print(f" Thread {thread_id}: {count} reads") - print("=============================\n") - - def reset_stats(self) -> None: - """重置所有統計數據""" - with self._stats_lock: - self.concurrent_write_count.value = 0 - self.concurrent_read_count.value = 0 - self.total_write_count.value = 0 - self.total_read_count.value = 0 - self.overflow_count.value = 0 - self.thread_write_counts.clear() - self.thread_read_counts.clear() - - def __str__(self) -> str: - """返回緩衝區的字符串表示""" - return f"RingBuffer(id={self.buffer_id}, capacity={self.capacity}, size={self.size()})" \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/serialManager.py b/src/fc_network_adapter/fc_network_adapter/serialManager.py index 731a950..883af90 100644 --- a/src/fc_network_adapter/fc_network_adapter/serialManager.py +++ b/src/fc_network_adapter/fc_network_adapter/serialManager.py @@ -15,6 +15,7 @@ import time import threading import struct from enum import Enum, auto +from abc import ABC, abstractmethod # # XBee 模組 # from xbee.frame import APIFrame @@ -27,6 +28,105 @@ from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) # ====================== 分割線 ===================== + +# 定義 serial 連線的模式 +class SerialMode(Enum): + """連接類型""" + STRAIGHT = auto() # 原始數據直通 + XBEEAPI2AT = auto() # XBee API 模式 + NOT_USE = auto() # 不使用 + + +# ====================== Frame Processor 基類與實現 ===================== + +class FrameProcessor(ABC): + """協議處理器基類""" + + def __init__(self): + self.buffer = bytearray() + + @abstractmethod + def process_incoming(self, data: bytes): + """ + 處理接收到的數據 + 返回：已完整解析的 payload 列表 + """ + pass + + @abstractmethod + def process_outgoing(self, data: bytes) -> bytes: + """ + 封裝要發送的數據 + 返回：封裝後的完整幀 + """ + pass + + +class RawFrameProcessor(FrameProcessor): + """原始數據直通處理器""" + + def process_incoming(self, data: bytes): + """直接返回原始數據，不進行緩衝""" + return [data] if data else [] + + def process_outgoing(self, data: bytes) -> bytes: + """直接返回原始數據，不進行封裝""" + return data + + +class XBeeFrameProcessor(FrameProcessor): + """XBee API 協議處理器""" + + def process_incoming(self, data: bytes): + """處理 XBee API 幀並提取 payload""" + self.buffer.extend(data) + payloads = [] + + while len(self.buffer) >= 3: + # 尋找幀頭 + if self.buffer[0] != 0x7E: + self.buffer.pop(0) + continue + + # 讀取 payload 長度 + length = (self.buffer[1] << 8) | self.buffer[2] + full_length = 3 + length + 1 # 起始符(1) + 長度(2) + payload + 校驗和(1) + + # 等待完整幀 + if len(self.buffer) < full_length: + break + + # 提取完整 frame 並從緩衝區移除 + frame = bytes(self.buffer[:full_length]) + del self.buffer[:full_length] + + # 判斷 frame 類型並處理 + frame_type = frame[3] + + if frame_type == 0x90: # RX Packet + payload = XBeeFrameHandler.decapsulate_data(frame) + if payload: + payloads.append(payload) + elif frame_type == 0x88: # AT Response + # 可以在這裡處理 AT 指令回應 + # response = XBeeFrameHandler.parse_at_command_response(frame) + # 目前忽略 + pass + elif frame_type == 0x8B: # Transmit Status + # 傳輸狀態，目前忽略 + pass + else: + logger.warning(f"Unknown XBee frame type: 0x{frame_type:02X}") + + return payloads + + def process_outgoing(self, data: bytes) -> bytes: + """將數據封裝為 XBee API 傳輸幀""" + return XBeeFrameProcessor.encapsulate_data(data) + + +# ====================== XBee Frame Handler ===================== + class XBeeFrameHandler: """XBee API Frame 處理器""" @@ -55,7 +155,7 @@ class XBeeFrameHandler: @staticmethod def parse_receive_packet(frame: bytes) -> dict: - # """解析 RX Packet (0x90) - 未來擴展用""" + """解析 RX Packet (0x90) - 未來擴展用""" # if len(frame) < 15 or frame[3] != 0x90: # return None @@ -91,34 +191,12 @@ class XBeeFrameHandler: @staticmethod def decapsulate_data(data: bytes): - # 這裡可以根據需要進行數據解封裝 - - # XBee API 幀格式: - # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) - # 檢查幀起始符 (0x7E) - if not data or len(data) < 5 or data[0] != 0x7E: - return data - # 獲取數據長度 (不包括校驗和) - # length = (data[1] << 8) + data[2] length = (data[1] << 8) | data[2] - - # 檢查幀完整性 - if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 - return data - - # 提取API標識符和數據 - frame_type = data[3] - # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 - - # 根據不同的幀類型進行處理 - if frame_type == 0x90: # 例如，這是"接收數據包"類型 - rf_data_start = 3 + 12 - return data[rf_data_start:3 + length] - else: - return None - return data + + rf_data_start = 3 + 12 + return data[rf_data_start:3 + length] class ATCommandHandler: @@ -137,7 +215,7 @@ class ATCommandHandler: """根據 AT 指令類型分派處理""" if not response or not response['is_ok']: if response: - print(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}") + logger.warning(f"[{self.serial_port}] AT {response['command'].decode()} 失敗，狀態碼: {response['status']}") return command = response['command'] @@ -146,189 +224,122 @@ class ATCommandHandler: if handler: handler(response['data']) else: - print(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}") + logger.debug(f"[{self.serial_port}] 未處理的 AT 指令: {command.decode()}") def _handle_rssi(self, data: bytes): """處理 DB (RSSI) 回應""" - if not data: - return - - rssi_value = data[0] - now = time.time() - - # 檢查是否最近有收到 MAVLink - last_mavlink_time = serial_last_mavlink_time.get(self.serial_port, 0) - if now - last_mavlink_time > 0.5: - print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") - return - - # 取得對應的 sysid - sysid = serial_to_sysid.get(self.serial_port) - if sysid is None: - print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") - return - - # 記錄 RSSI - rssi_history[sysid].append(-rssi_value) - time_history[sysid].append(now) - # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") + # 未來可實現 RSSI 處理邏輯 + pass def _handle_serial_high(self, data: bytes): - # """處理 SH (Serial Number High) - 範例""" - # if len(data) >= 4: - # serial_high = int.from_bytes(data[:4], 'big') - # print(f"[{self.serial_port}] Serial High: 0x{serial_high:08X}") + """處理 SH (Serial Number High)""" pass def _handle_serial_low(self, data: bytes): - # """處理 SL (Serial Number Low) - 範例""" - # if len(data) >= 4: - # serial_low = int.from_bytes(data[:4], 'big') - # print(f"[{self.serial_port}] Serial Low: 0x{serial_low:08X}") + """處理 SL (Serial Number Low)""" pass -# ====================== 分割線 ===================== -class SerialHandler(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 - def __init__(self, udp_handler, serial_port_str): - self.udp_handler = udp_handler # UDP 的傳輸物件 - self.serial_port_str = serial_port_str - self.at_handler = ATCommandHandler(serial_port_str) +# ====================== Serial Handler ===================== - self.buffer = bytearray() # 用於緩存接收到的資料 - self.transport = None # Serial 自己的傳輸物件 - # self.first_data = True # 標記是否為第一次收到資料 - # self.has_processed = False # 測試模式用 處理數據旗標 # debug +class SerialHandler(asyncio.Protocol): + """asyncio.Protocol 用於處理 Serial 收發""" + + def __init__(self, udp_handler, serial_port_str, serial_mode: SerialMode): + self.udp_handler = udp_handler # UDP 的傳輸物件 + self.serial_port_str = serial_port_str + self.serial_mode = serial_mode + self.transport = None # Serial 自己的傳輸物件 + + # 根據模式創建對應的 processor + self.processor = self._create_processor(serial_mode) + + # AT 指令處理器（僅 XBee 模式使用） + if serial_mode == SerialMode.XBEEAPI2AT: + self.at_handler = ATCommandHandler(serial_port_str) + else: + self.at_handler = None + + def _create_processor(self, serial_mode: SerialMode) -> FrameProcessor: + """工廠方法：根據模式創建處理器""" + if serial_mode == SerialMode.STRAIGHT: + return RawFrameProcessor() + elif serial_mode == SerialMode.XBEEAPI2AT: + return XBeeFrameProcessor() + else: + logger.warning(f"Unknown serial mode: {serial_mode}, using Raw") + return RawFrameProcessor() def connection_made(self, transport): + """連接建立時的回調""" self.transport = transport if hasattr(self.udp_handler, 'set_serial_handler'): self.udp_handler.set_serial_handler(self) - # logger.info(f"Serial port {self.serial_port_str} connected.") # debug + logger.debug(f"Serial port {self.serial_port_str} connected") - # Serial 收到資料的處理過程 def data_received(self, data): - # 1. 把收到的資料加入緩衝區 - self.buffer.extend(data) - - # 2. 需要完整的 header 才能解析 - while len(self.buffer) >= 3: - # 3. 瞄準 XBee API Frame (0x7E 開頭的封包) - if self.buffer[0] != 0x7E: - self.buffer.pop(0) # 如果不是就丟掉 - continue - - # 4. 讀取 payload 長度 - length = (self.buffer[1] << 8) | self.buffer[2] - full_length = 3 + length + 1 - - # 5. 等待完整封包 - if len(self.buffer) < full_length: - break + """Serial 收到資料的處理過程""" + # 使用 processor 處理接收到的數據 + payloads = self.processor.process_incoming(data) + + # 將所有解析完成的 payload 轉發到 UDP + for payload in payloads: + self.udp_handler.transport.sendto( + payload, + (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port) + ) - # 6. 提取完整 frame 並從緩衝區移除 - an_frame = self.buffer[:full_length] - del self.buffer[:full_length] - # 7. 判斷 frame 類型 - frame_type = an_frame[3] - - if frame_type == 0x88: - # 處理 AT Command 回應 - # response = XBeeFrameHandler.parse_at_command_response(an_frame) - # self.at_handler.handle_response(response) - pass - - elif frame_type == 0x90: - # Receive Packet (RX) payload 先解碼 - processed_data = XBeeFrameHandler.decapsulate_data(bytes(an_frame)) - # 轉換失敗就捨棄了 - if processed_data is None: - continue - # 再透過 UDP 送出 - self.udp_handler.transport.sendto(processed_data, (self.udp_handler.LOCAL_HOST_IP, self.udp_handler.target_port)) - - elif frame_type == 0x8B: - pass +# ====================== UDP Handler ===================== - else: - # 其他類型的 frame 未來可擴展處理 現在忽略 - logger.warning(f"[{self.serial_port_str}] Undefined frame type: 0x{frame_type:02X}") - - # # RSSI - # if frame[3] == 0x88 and frame[5:7] == b'DB': # frame[3] == 0x88 AT -> API 封包 - # # frame[5:7] == b'DB' -> API 封包的DB參數 - # status = frame[7] # - # if status == 0x00 and len(frame) > 8: # status == 0x00 -> 這個封包是有效封包 - # rssi_value = frame[8] - # now = time.time() - - # # === 優化 1：僅信任最近 0.5 秒內有接收 MAVLink 的 port - # last_time = serial_last_mavlink_time.get(self.serial_port, 0) - # if now - last_time <= 0.5: - # sysid = serial_to_sysid.get(self.serial_port, None) - # if sysid is not None: - # rssi_history[sysid].append(-rssi_value) - # time_history[sysid].append(now) - # # print(f"[SYSID:{sysid}] RSSI = -{rssi_value} dBm") - # else: - # print(f"[{self.serial_port}] 找不到 sysid 對應，RSSI = -{rssi_value} dBm，已忽略") - # else: - # print(f"[{self.serial_port}] 超過 0.5 秒未接收 MAVLink，RSSI = -{rssi_value} dBm 已忽略") - # else: - # print(f"[{self.serial_port}] DB 指令失敗，狀態碼: {status}") - - -class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 +class UDPHandler(asyncio.DatagramProtocol): + """asyncio.DatagramProtocol 用於處理 UDP 收發""" - LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端IP - - def __init__(self, target_port): - self.target_port = target_port # 目標 UDP 端口 + LOCAL_HOST_IP = '127.0.0.1' # 只送給本地端 IP - self.serial_handler = None # Serial 的傳輸物件 - self.transport = None # UDP 自己的傳輸物件 - self.remote_addr = None # 儲存動態獲取的遠程地址 # debug - # self.has_processed = False # 測試模式用 處理數據旗標 # debug + def __init__(self, target_port, serial_mode: SerialMode): + self.target_port = target_port # 目標 UDP 端口 + self.serial_mode = serial_mode + self.serial_handler = None # Serial 的傳輸物件 + self.transport = None # UDP 自己的傳輸物件 def connection_made(self, transport): + """連接建立時的回調""" self.transport = transport - # logger.info(f"UDP transport ready. Waiting for serial data before sending.") # debug + logger.debug(f"UDP transport ready for port {self.target_port}") def set_serial_handler(self, serial_handler): + """設置對應的 Serial Handler""" self.serial_handler = serial_handler - # UDP 收到資料的處理過程 def datagram_received(self, data, addr): - # 儲存對方的地址（這樣就能向同一個來源回傳數據） - # self.remote_addr = addr # debug - # print(f"Received UDP data from {addr}, setting as remote address") + """UDP 收到資料的處理過程""" + if not self.serial_handler: + logger.warning("Serial handler not set, dropping UDP packet") + return - processed_data = XBeeFrameHandler.encapsulate_data(data) - - if self.serial_handler: - self.serial_handler.transport.write(processed_data) - -#================================================================== + # 使用 processor 封裝數據 + processed_data = self.serial_handler.processor.process_outgoing(data) + + # 發送到串口 + self.serial_handler.transport.write(processed_data) -class SerialReceiverType(Enum): - """連接類型""" - TELEMETRY = auto() - XBEEAPI2AT = auto() - OTHER = auto() +# ====================== Serial Manager ===================== class serial_manager: + """串口管理器""" class serial_object: - def __init__(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): - self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc + """串口物件""" + def __init__(self, serial_port, baudrate, target_port, serial_mode: SerialMode): + self.serial_port = serial_port # /dev/ttyUSB or COM3 ...etc self.baudrate = baudrate - self.receiver_type = receiver_type - self.target_port = target_port # 指向的 UPD 端口 + self.serial_mode = serial_mode + self.target_port = target_port # 指向的 UDP 端口 - self.transport = None # TODO 這個變數可能沒有作用 - self.protocol = None # TODO 這個變數可能沒有作用 + self.transport = None + self.protocol = None self.udp_handler = None self.serial_handler = None @@ -337,21 +348,21 @@ class serial_manager: self.loop = None self.running = False self.serial_count = 0 - self.serial_objects = {} # serial id num : serial_object + self.serial_objects = {} # serial id num : serial_object def __del__(self): self.loop = None self.thread = None def start(self): - + """啟動 serial_manager""" if self.running: logger.warning("serial_manager already running") - return + return False self.running = True - # 啟動獨立線程 命名為 SerialManager + # 啟動獨立線程，命名為 SerialManager self.thread = threading.Thread( target=self._run_event_loop, name="SerialManager" @@ -375,7 +386,6 @@ class serial_manager: def shutdown(self): """停止 serial_manager 和其管理的所有 serial_object""" - # 自己在 running 狀態下才執行停止程序 if not self.running: logger.warning("serial_manager is not running") return @@ -404,37 +414,25 @@ class serial_manager: self.loop = asyncio.new_event_loop() asyncio.set_event_loop(self.loop) - # # 為每個 serial_object 建立連接 - # for serial_obj in self.serial_objects: - # coro = serial_asyncio.create_serial_connection( - # self.loop, - # lambda: SerialProtocol(serial_obj.receiver_type), - # serial_obj.serial_port, - # baudrate=serial_obj.baudrate - # ) - # transport, protocol = self.loop.run_until_complete(coro) - # serial_obj.transport = transport - # serial_obj.protocol = protocol - try: self.loop.run_forever() finally: self.loop.close() - def create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): - + def create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode): + """創建串口連接""" if not self.running or not self.loop: logger.error("Event loop not running, cannot create serial link") return False - # 檢查 serial port 有效 + # 檢查 serial port 有效性 if not self.check_serial_port(serial_port, baudrate): logger.error(f"Serial port {serial_port} validation failed") return False # 使用 run_coroutine_threadsafe 執行協程並獲取結果 future = asyncio.run_coroutine_threadsafe( - self._async_create_serial_link(serial_port, baudrate, target_port, receiver_type), + self._async_create_serial_link(serial_port, baudrate, target_port, serial_mode), self.loop ) @@ -442,8 +440,8 @@ class serial_manager: # 等待結果，設定合理的超時時間 result = future.result(timeout=5.0) if result: - logger.info(f"Create Serial Link: {serial_port} -> UDP {target_port}") - return True + logger.info(f"Create Serial Link: {serial_port} ({serial_mode.name}) -> UDP {target_port}") + return result except asyncio.TimeoutError: logger.error(f"Timeout creating serial link for {serial_port}") return False @@ -451,27 +449,28 @@ class serial_manager: logger.error(f"Failed to create serial link for {serial_port}: {e}") return False - async def _async_create_serial_link(self, serial_port, baudrate, target_port, receiver_type: SerialReceiverType): + async def _async_create_serial_link(self, serial_port, baudrate, target_port, serial_mode: SerialMode): """在事件循環線程中執行實際的連接創建""" try: # 創建 serial_object 實例 - serial_obj = self.serial_object(serial_port, baudrate, target_port, receiver_type) + serial_obj = self.serial_object(serial_port, baudrate, target_port, serial_mode) # 建立 UDP 處理器並指定目標端口位置 - serial_obj.udp_handler = UDPHandler(target_port) + serial_obj.udp_handler = UDPHandler(target_port, serial_mode) # 建立 UDP 傳輸，不指定接收端口(自己)，讓系統自動分配 udp_transport, udp_protocol = await self.loop.create_datagram_endpoint( lambda: serial_obj.udp_handler, local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 ) + serial_obj.transport = udp_transport serial_obj.protocol = udp_protocol - # logger.info(f"UDP endpoint created for {serial_port}") # debug + logger.debug(f"UDP endpoint created for {serial_port}") - # 建立 Serial 處理器，將 UDP 處理器傳給它 - serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port) + # 建立 Serial 處理器，將 UDP 處理器和模式傳給它 + serial_obj.serial_handler = SerialHandler(serial_obj.udp_handler, serial_port, serial_mode) # 建立 Serial 連接 serial_transport, _ = await serial_asyncio.create_serial_connection( @@ -481,14 +480,14 @@ class serial_manager: baudrate=baudrate ) - # logger.info(f"Serial connection created for {serial_port}") # debug + logger.debug(f"Serial connection created for {serial_port}") # 將 serial_object 加入管理列表 serial_id = self.serial_count + 1 self.serial_objects[serial_id] = serial_obj self.serial_count += 1 - # logger.info(f"Serial object {serial_id} added to manager") # debug + logger.debug(f"Serial object {serial_id} added to manager") return True except Exception as e: @@ -501,12 +500,10 @@ class serial_manager: def remove_serial_link(self, serial_id): """移除串口連接（線程安全方式）""" - # 確保事件循環正在運行 if not self.loop: logger.error("Event loop not running") return False - # 檢查 serial_id 是否存在 if serial_id not in self.serial_objects: logger.warning(f"Serial object {serial_id} not found") return False @@ -549,7 +546,7 @@ class serial_manager: # 從管理列表中移除 del self.serial_objects[serial_id] - # logger.info(f"Serial object {serial_id} removed from manager") # debug + logger.debug(f"Serial object {serial_id} removed from manager") return True except Exception as e: @@ -557,7 +554,8 @@ class serial_manager: return False def get_serial_link(self): - ret = {} # serial id num : serial_port string + """取得所有串口連接資訊""" + ret = {} # serial id num : serial_port string for key, obj in self.serial_objects.items(): ret[key] = obj.serial_port return ret @@ -593,6 +591,8 @@ class serial_manager: return False +# ====================== 測試代碼 ===================== + if __name__ == '__main__': sm = serial_manager() sm.start() @@ -600,12 +600,17 @@ if __name__ == '__main__': SERIAL_PORT = '/dev/ttyUSB0' # 手動指定 SERIAL_BAUDRATE = 115200 UDP_REMOTE_PORT = 14571 - sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialReceiverType.XBEEAPI2AT) + sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.XBEEAPI2AT) + + # SERIAL_PORT = '/dev/ttyACM0' # 手動指定 + # SERIAL_BAUDRATE = 115200 + # UDP_REMOTE_PORT = 14571 + # sm.create_serial_link(SERIAL_PORT, SERIAL_BAUDRATE, UDP_REMOTE_PORT, SerialMode.STRAIGHT) linked_serial = sm.get_serial_link() print(linked_serial) - time.sleep(10) + time.sleep(60) sm.remove_serial_link(1) time.sleep(3) - sm.shutdown() \ No newline at end of file + sm.shutdown() diff --git a/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py b/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py deleted file mode 100644 index cb4373c..0000000 --- a/src/fc_network_adapter/fc_network_adapter/serial_udp_bitrans.py +++ /dev/null @@ -1,340 +0,0 @@ -import asyncio -import serial_asyncio - -# 附加驗證功能 -import os -import sys -import serial -import signal - -# XBee 模組 -from xbee.frame import APIFrame - -# ========================= - -SERIAL_PORT = '/dev/ttyACM0' # serial port -SERIAL_BAUDRATE = 57600 # serial baudrate - -UDP_REMOTE_IP = '127.0.0.1' # UDP Target IP -UDP_REMOTE_PORT = 14550 # UDP Target port - -# 測試用 只會吃一次資料 -DEBUG_MODE = False - -# ========================= - -def check_serial_port(): - """檢查串口是否存在與可用""" - # 檢查設備是否存在 - if not os.path.exists(SERIAL_PORT): - print(f"錯誤：串口設備 {SERIAL_PORT} 不存在") - return False - - # 檢查是否有權限訪問設備 - try: - os.access(SERIAL_PORT, os.R_OK | os.W_OK) - except Exception as e: - print(f"錯誤：無法訪問串口設備 {SERIAL_PORT}：{str(e)}") - return False - - # 檢查是否被占用 - try: - # 嘗試打開串口 - ser = serial.Serial(SERIAL_PORT, SERIAL_BAUDRATE) - ser.close() # 打開成功後立即關閉 - return True - except serial.SerialException as e: - print(f"錯誤：串口設備 {SERIAL_PORT} 被占用或無法訪問：{str(e)}") - return False - except Exception as e: - print(f"錯誤：檢查串口時發生未知錯誤：{str(e)}") - return False - -# ========================= - -class SerialToUDP(asyncio.Protocol): # asyncio.Protocol 用於處理 Serial 收發 - def __init__(self, udp_protocol): - self.udp_protocol = udp_protocol - self.first_data = True # 標記是否為第一次收到資料 - self.has_processed = False # 測試模式用 處理數據旗標 # debug - - def connection_made(self, transport): - self.transport = transport - if hasattr(self.udp_protocol, 'set_serial_transport'): - self.udp_protocol.set_serial_transport(self) - print(f"Serial connection established on {SERIAL_PORT}") - - ## ===================================== - - # Serial 收到資料，轉發到 UDP - def data_received(self, data): - # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 # debug - if DEBUG_MODE and self.has_processed: - return - - # 標記首次收到資料 - if hasattr(self.udp_protocol, 'send_udp'): - if self.first_data: - print(f"First data received from serial, forwarding to UDP: {data[:20]}...") - self.first_data = False - - # 轉發數據 - self.udp_protocol.send_udp(data) - - if DEBUG_MODE: # 測試模式用 # debug - self.has_processed = True - print("[DEBUG] SerialToUDP Mark") - - def write_to_serial(self, data): - # 在資料透過 Serial 發送之前進行處理 - processed_data = self.encapsulate_data(data) - - # 處理失敗就不發送 - if processed_data not None: - self.transport.write(processed_data) - - def encapsulate_data(self, data): - - """ - 將數據封裝為 XBee API 傳輸幀 - - 使用 XBee API 格式封裝數據: - - 傳輸請求幀 (0x10) - - 使用廣播地址 - - 添加適當的頭部和校驗和 - """ - - ## 方法一 - # 設置幀參數 - frame_id = 0x01 # 幀識別碼，用於確認 - frame_type = 0x10 # 幀類型：傳輸請求 - dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 64位目標地址 (廣播) - dest_addr16 = b'\xFF\xFE' # 16位目標地址 (未知/廣播) - broadcast_radius = 0x00 # 廣播半徑 (0 = 最大) - options = 0x00 # 傳輸選項 - - # 構建幀數據部分 - frame_data = bytearray() - frame_data.append(frame_type) # 添加幀類型 - frame_data.append(frame_id) # 添加幀 ID - frame_data.extend(dest_addr64) # 添加 64 位目標地址 - frame_data.extend(dest_addr16) # 添加 16 位目標地址 - frame_data.append(broadcast_radius) # 添加廣播半徑 - frame_data.append(options) # 添加選項 - frame_data.extend(data) # 添加實際數據內容 - - # 計算校驗和 (0xFF 減去所有字節的總和的最低 8 位) - checksum = 0xFF - (sum(frame_data) & 0xFF) - - # 構建完整的 API 幀 - # 起始分隔符 + 長度 (兩字節) + 幀數據 + 校驗和 - complete_frame = bytearray() - complete_frame.append(0x7E) # 添加起始分隔符 - complete_frame.extend(struct.pack(">H", len(frame_data))) # 添加長度 (高位優先) - complete_frame.extend(frame_data) # 添加幀數據 - complete_frame.append(checksum) # 添加校驗和 - - return bytes(complete_frame) - - ## 方法二 - # frame_id=0x01 - # frame_type = 0x10 - # dest_addr64 = b'\x00\x00\x00\x00\x00\x00\xFF\xFF' # 廣播 - # dest_addr16 = b'\xFF\xFE' - # broadcast_radius = 0x00 - # options = 0x00 - - # frame = struct.pack(">B", frame_type) + struct.pack(">B", frame_id) - # frame += dest_addr64 + dest_addr16 - # frame += struct.pack(">BB", broadcast_radius, options) + data - # checksum = 0xFF - (sum(frame) & 0xFF) - # return b'\x7E' + struct.pack(">H", len(frame)) + frame + struct.pack("B", checksum) - - -class UDPHandler(asyncio.DatagramProtocol): # asyncio.DatagramProtocol 用於處理 UDP 收發 - def __init__(self): - self.serial_transport = None - self.transport = None - self.remote_addr = None # 儲存動態獲取的遠程地址 - self.has_processed = False # 測試模式用 處理數據旗標 # debug - - def connection_made(self, transport): - self.transport = transport - print("UDP transport ready. Waiting for serial data before sending...") - - def set_serial_transport(self, serial_transport): - self.serial_transport = serial_transport - - ## ===================================== - - # UDP 收到資料 - def datagram_received(self, data, addr): - # 儲存對方的地址（這樣就能向同一個來源回傳數據） - # self.remote_addr = addr - # print(f"Received UDP data from {addr}, setting as remote address") - - # 在 DEBUG 模式下，如果已經處理過數據，則直接返回 - if DEBUG_MODE and self.has_processed: - return - - if self.serial_transport: - self.serial_transport.write_to_serial(data) - - if DEBUG_MODE: # 測試模式用 - self.has_processed = True - print("[DEBUG] UDPHandler Mark") - - def send_udp(self, data): - # 發送資料到 UDP - - # if self.transport: - # # 如果有已知的回應地址，使用該地址 - # if self.remote_addr: - # self.transport.sendto(data, self.remote_addr) - # # print(f"Sending to dynamic address: {self.remote_addr}") - # else: - # # 否則使用預設地址 - # self.transport.sendto(data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) - # print(f"Sending first UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") - - if self.transport: - # 只有第一次或地址改變時才顯示 - # if not hasattr(self, '_last_sent_addr') or self._last_sent_addr != (UDP_REMOTE_IP, UDP_REMOTE_PORT): - # print(f"Sending UDP packet to: {UDP_REMOTE_IP}:{UDP_REMOTE_PORT}") - # self._last_sent_addr = (UDP_REMOTE_IP, UDP_REMOTE_PORT) - - # 在透過 UDP 發送數據之前進行解封裝 - decoded_data = self.decapsulate_data(data) - self.transport.sendto(decoded_data, (UDP_REMOTE_IP, UDP_REMOTE_PORT)) - - def decapsulate_data(self, data): - # 這裡可以根據需要進行數據解封裝 - - ## 方法一 - try: - # 創建一個 API 幀處理器 - api_frame = APIFrame(escaped=True) - - # 嘗試解析數據 - api_frame.fill(data) - - # 如果數據不完整，直接返回原始數據 - if not api_frame.is_complete(): - return data - - # 解析幀內容 - parsed_data = api_frame.parse() - - # 提取有用數據 - if parsed_data: - frame_data = parsed_data.get('rf_data', None) - if frame_data: - return frame_data - - return data - - ## 方法二 - 手動解析 - # try: - # # XBee API 幀格式: - # # 起始分隔符(1字節) + 長度(2字節) + API標識符(1字節) + 數據 + 校驗和(1字節) - - # # 檢查幀起始符 (0x7E) - # if not data or len(data) < 5 or data[0] != 0x7E: - # return data - - # # 獲取數據長度 (不包括校驗和) - # length = (data[1] << 8) + data[2] - - # # 檢查幀完整性 - # if len(data) < length + 4: # 起始符 + 長度(2字節) + 數據 + 校驗和 - # return data - - # # 提取API標識符和數據 - # frame_type = data[3] - # frame_data = data[4:4+length-1] # 減1是因為API標識符已經算在長度中 - - # # 根據不同的幀類型進行處理 - # if frame_type == 0x90: # 例如，這是"接收數據包"類型 - # # 提取實際有效載荷 - # # 對於接收數據包，格式通常是： - # # API標識符(1) + 64位地址(8) + 16位地址(2) + 選項(1) + 數據 - # if len(frame_data) >= 12: # 確保數據長度足夠 - # payload = frame_data[11:] # 前11字節是地址和選項 - # return payload - # return data - - # 如果無法提取 則回傳 None - except Exception as e: - print(f"手動解析 XBee 數據錯誤: {e}") - return None - -async def main(): - # 先檢查串口是否可用 - if not check_serial_port(): - print("程式終止：串口檢查失敗") - return - - loop = asyncio.get_running_loop() - - # 設置終止處理 - for sig in (signal.SIGINT, signal.SIGTERM): - loop.add_signal_handler( - sig, - lambda: asyncio.create_task(shutdown(loop)) - ) - - # 建立單一 UDP 端點處理收發 - udp_handler = UDPHandler() - - # 建立 UDP 傳輸，不指定接收端口，讓系統自動分配 - try: - udp_transport, protocol = await loop.create_datagram_endpoint( - lambda: udp_handler, - local_addr=('0.0.0.0', 0) # 使用端口 0 讓系統自動分配可用端口 - ) - except Exception as e: - print(f"無法創建 UDP 端點：{str(e)}") - return - - # 獲取系統分配的本地端口 - sock = udp_transport.get_extra_info('socket') - local_addr = sock.getsockname() - print(f"UDP listening on {local_addr[0]}:{local_addr[1]}") - - # 建立 Serial 傳輸，將 UDP 處理器傳給它 - try: - serial_proto = SerialToUDP(udp_handler) - _, serial_transport = await serial_asyncio.create_serial_connection( - loop, lambda: serial_proto, SERIAL_PORT, baudrate=SERIAL_BAUDRATE - ) - except Exception as e: - print(f"無法建立串口連接：{str(e)}") - udp_transport.close() - return - - print(f"Waiting for data from serial port to initiate UDP communication...") - - # 保持運行 - try: - await asyncio.Future() - except asyncio.CancelledError: - pass - -async def shutdown(loop): - """關閉程序""" - print("Shutting down...") - tasks = [t for t in asyncio.all_tasks() if t is not asyncio.current_task()] - - for task in tasks: - task.cancel() - - await asyncio.gather(*tasks, return_exceptions=True) - loop.stop() - -if __name__ == '__main__': - try: - asyncio.run(main()) - except KeyboardInterrupt: - print("程式被使用者中斷") - except Exception as e: - print(f"程式執行錯誤：{str(e)}") \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/test_mavlinkObject.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/fc_network_adapter/fc_network_adapter/theLogger.py b/src/fc_network_adapter/fc_network_adapter/theLogger.py deleted file mode 100644 index c4dccfe..0000000 --- a/src/fc_network_adapter/fc_network_adapter/theLogger.py +++ /dev/null @@ -1,43 +0,0 @@ -import logging -import os -from logging.handlers import TimedRotatingFileHandler - -# 全域 Logger 實例 -_global_logger = None - -def setup_logger(name: str, log_dir: str = "log", level=logging.DEBUG) -> logging.Logger: - global _global_logger - - if _global_logger is None: - # 確保 logs 資料夾存在 - os.makedirs(log_dir, exist_ok=True) - - # 建立全域 Logger - _global_logger = logging.getLogger("global_logger") - _global_logger.setLevel(level) - _global_logger.propagate = False # 防止重複輸出 - - formatter = logging.Formatter( - fmt="%(asctime)s | %(levelname)s | %(name)s | %(message)s", - datefmt="%m-%d %H:%M:%S" - ) - - # 檔案輸出（每天輪替） - file_handler = TimedRotatingFileHandler( - filename=os.path.join(log_dir, "application.log"), - when="midnight", # 每天 0 點輪替 - backupCount=7, # 保留 7 天 - encoding="utf-8" - ) - file_handler.setFormatter(formatter) - _global_logger.addHandler(file_handler) - - # 終端機輸出 - console_handler = logging.StreamHandler() - console_handler.setFormatter(formatter) - _global_logger.addHandler(console_handler) - - # 為每個模組建立子 Logger，並設定名稱 - module_logger = _global_logger.getChild(name) - module_logger.name = name # 修改子 Logger 的名稱，僅保留子 Logger 名稱 - return module_logger diff --git a/src/fc_network_adapter/tests/devRun.py b/src/fc_network_adapter/tests/devRun.py deleted file mode 100644 index 1544ebf..0000000 --- a/src/fc_network_adapter/tests/devRun.py +++ /dev/null @@ -1,448 +0,0 @@ -import os -import sys -sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), '..'))) - -# 基礎功能的 import -import queue -import time - -# ROS2 的 import -import rclpy - -# mavlink 的 import -from pymavlink import mavutil - -# 自定義的 import -from fc_network_adapter import mavlinkObject as mo -from fc_network_adapter import mavlinkDevice as md - -# ====================== 分割線 ===================== - -test_item = 12 -running_time = 500 - - -print('test_item : ', test_item) - -''' -測試項 個位數 表示分離的功能 - -測試項 1X 表示 mavlink_object 的功能 測試連線的能力 -''' - -if test_item == 10: - # 需要開啟一個 ardupilot 的模擬器 - # 測試 mavlink_object 放入 ring buffer 的應用 - print('===> Start of Program .Test ', test_item) - - # 清空 ring buffer - mo.stream_bridge_ring.clear() - mo.return_packet_ring.clear() - - manager = mo.async_io_manager() - manager.start() - time.sleep(0.5) # 等待事件循環啟動 - - # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" - mavlink_socket1 = mavutil.mavlink_connection(connection_string) - mavlink_object1 = mo.mavlink_object(mavlink_socket1) - - manager.add_mavlink_object(mavlink_object1) - - start_time = time.time() - while (time.time() - start_time) < running_time: - items_a = mo.stream_bridge_ring.get_all() - items_b = mo.return_packet_ring.get_all() - try: - print(f"data num {len(items_a)} in return_packet_ring, first data : {items_a[0][2]}") - except IndexError: - print("stream_bridge_ring is empty") - - try: - print(f"data num {len(items_b)} in return_packet_ring, first data : {items_b[0][2]}") - except IndexError: - print("return_packet_ring is empty") - time.sleep(1) - - manager.stop() - - print('<=== End of Program') - -elif test_item == 11: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況 - print('===> Start of Program .Test ', test_item) - - # 清空 ring buffer - mo.stream_bridge_ring.clear() - mo.return_packet_ring.clear() - - manager = mo.async_io_manager() - manager.start() - time.sleep(0.5) # 等待事件循環啟動 - - # 初始化輸入通道 - connection_string="udp:127.0.0.1:14560" - mavlink_socket1 = mavutil.mavlink_connection(connection_string) - mavlink_object1 = mo.mavlink_object(mavlink_socket1) - manager.add_mavlink_object(mavlink_object1) - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - - time.sleep(3) - - # 印出目前所有 mavlink_systems 的內容 - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - if (time.time() - show_time) >= 2: - # print("mark B") - - show_time = time.time() - for sysid in analyzer.mavlink_systems: - for compid in analyzer.mavlink_systems[sysid].components: - print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) - analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) - print("===================") - - manager.stop() - analyzer.stop() - - print('<=== End of Program') - -elif test_item == 12: - # 需要開啟一個 ardupilot 的模擬器 與 GCS - # 這邊是測試 mavlink object 作為交換器功能的代碼 - print('===> Start of Program .Test ', test_item) - - # 清空 ring buffer - mo.stream_bridge_ring.clear() - mo.return_packet_ring.clear() - - manager = mo.async_io_manager() - manager.start() - time.sleep(0.5) # 等待事件循環啟動 - - # 初始化輸入通道 - connection_string="udp:127.0.0.1:14554" - mavlink_socket_in1 = mavutil.mavlink_connection(connection_string) - mavlink_object_in1 = mo.mavlink_object(mavlink_socket_in1) - - connection_string="udp:127.0.0.1:14551" - mavlink_socket_in2 = mavutil.mavlink_connection(connection_string) - mavlink_object_in2 = mo.mavlink_object(mavlink_socket_in2) - - # 初始化輸出通道 - connection_string="udpout:127.0.0.1:14553" - mavlink_socket_out = mavutil.mavlink_connection(connection_string) - mavlink_object_out = mo.mavlink_object(mavlink_socket_out) - - manager.add_mavlink_object(mavlink_object_out) - manager.add_mavlink_object(mavlink_object_in1) - manager.add_mavlink_object(mavlink_object_in2) - - mavlink_object_in1.add_target_socket(mavlink_object_out.socket_id) - mavlink_object_out.add_target_socket(mavlink_object_in1.socket_id) - - mavlink_object_in2.add_target_socket(mavlink_object_out.socket_id) - mavlink_object_out.add_target_socket(mavlink_object_in2.socket_id) - - start_time = time.time() - while (time.time() - start_time) < running_time: - - time.sleep(1) - - manager.stop() - - print('<=== End of Program') - - -# elif test_item == 20: -# # 這邊測試 node 生成 topic 的功能 -# # 利用 空的通道 發出假的 heartbeat 封包 -# print('===> Start of Program .Test ', test_item) -# rclpy.init() # 注意要初始化 rclpy 才能使用 node - -# from pymavlink.dialects.v20 import common as mavlink2 - -# sysid = 1 -# compid = 1 - -# # 啟動 mavlink_bridge -# analyzer = mo.mavlink_bridge() - -# mav = mavlink2.MAVLink(None) -# mav.srcSystem = sysid -# mav.srcComponent = compid - -# # 這是一個 heartbeat 封包 -# fake_heartbeat = mavlink2.MAVLink_heartbeat_message( -# type=mavlink2.MAV_TYPE_QUADROTOR, -# autopilot=mavlink2.MAV_AUTOPILOT_ARDUPILOTMEGA, -# base_mode=3, -# custom_mode=0, -# system_status=0, -# mavlink_version=3 -# ) -# mavlink_bytes = fake_heartbeat.pack(mav) -# fake_heartbeat_msg = mav.parse_char(mavlink_bytes) - -# print(analyzer.mavlink_systems) - -# mo.fixed_stream_bridge_queue.put((0, 0, fake_heartbeat_msg)) # socket id, timestamp, data -# time.sleep(0.1) -# print(analyzer.mavlink_systems) - -# # ROS2 初始化 -# analyzer._init_node() - -# # 創建 ROS2 topic -# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid -# print("topic created") -# time.sleep(5) - -# # 丟出 topic -# analyzer.emit_info() - -# # 結束程式 -# analyzer.running = False -# analyzer.destroy_node() - -# rclpy.shutdown() -# analyzer.stop() -# analyzer.thread.join() -# print('<=== End of Program') - -elif test_item == 21: - # 需要開啟一個 ardupilot 的模擬器 - # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 - - print('===> Start of Program .Test ', test_item) - # 初始化 rclpy 才能使用 node - rclpy.init() - - # 清空 ring buffer - mo.stream_bridge_ring.clear() - mo.return_packet_ring.clear() - - manager = mo.async_io_manager() - manager.start() - - # 啟動 mavlink_bridge - analyzer = mo.mavlink_bridge() - # 關於 Node 的初始化 - show_time = time.time() - analyzer._init_node() # 初始化 node - print('初始化 node 完成 耗時 : ',time.time() - show_time) - - time.sleep(0.5) # 系統 Setup 完成 - - - # 創建通道 - connection_string="udp:127.0.0.1:14560" - mavlink_socket = mavutil.mavlink_connection(connection_string) - mavlink_object3 = mo.mavlink_object(mavlink_socket) - manager.add_mavlink_object(mavlink_object3) - - print('=== waiting for mavlink data ...') - time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - - print('目前所有的系統 : ') - for sysid in analyzer.mavlink_systems: - print(analyzer.mavlink_systems[sysid]) - - compid = 1 - sysid = 1 - start_time = time.time() - analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) - end_time = time.time() - print(f"Execution time for create_flightMode: {end_time - start_time} seconds") - - print("start emit info") - - start_time = time.time() - show_time = time.time() - while time.time() - start_time < running_time: - try: - # print(analyzer.mavlink_systems[sysid].components[compid].emitParams['flightMode_mode']) - analyzer.emit_info() # 這邊是測試 node 的運行 - time.sleep(1) - except KeyboardInterrupt: - break - - - # 程式結束 - analyzer.destroy_node() - rclpy.shutdown() - - # 結束程式 退出所有 thread - manager.stop() - analyzer.stop() - analyzer.thread.join() - - mavlink_socket.close() - print('<=== End of Program') - -# elif test_item == 22: -# # 需要開啟一個 ardupilot 的模擬器 與 GCS -# # 這邊是測試代碼 引入 rclpy 來測試 node 的運行 -# print('===> Start of Program .Test ', test_item) -# rclpy.init() # 注意要初始化 rclpy 才能使用 node - -# # 啟動 mavlink_bridge -# analyzer = mo.mavlink_bridge() -# # 關於 Node 的初始化 -# show_time = time.time() -# analyzer._init_node() # 初始化 node -# print('初始化 node 完成 耗時 : ',time.time() - show_time) - -# # 初始化兩個通道 -# connection_string_in="udp:127.0.0.1:15551" -# mavlink_socket_in = mavutil.mavlink_connection(connection_string_in) -# mavlink_object_in = mo.mavlink_object(mavlink_socket_in) -# mavlink_object_in.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147] - - -# connection_string_out="udpout:127.0.0.1:14553" -# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out) -# mavlink_object_out = mo.mavlink_object(mavlink_socket_out) -# mavlink_object_out.multiplexingToAnalysis = [] - - - -# # 讓兩個通道互相傳輸 -# mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all -# mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all - -# # 啟動通道 -# mavlink_object_in.run() -# mavlink_object_out.run() - -# print('waiting for mavlink data ...') -# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - -# print('目前所有的系統 : ') -# for sysid in analyzer.mavlink_systems: -# print(analyzer.mavlink_systems[sysid]) - -# compid = 1 -# sysid = 1 -# show_time = time.time() -# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# print(f"Execution time for create_flightMode: {time.time() - show_time} seconds") - -# print("start emit info") - -# start_time = time.time() -# show_time = time.time() -# show_time2 = time.time() - -# while time.time() - start_time < running_time: -# if (time.time() - show_time2) >= 1: -# analyzer.emit_info() # 這邊是測試 node 的運行 -# # sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode'] -# fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode'] -# sss = mavutil.mode_string_v10(fmsg) -# # print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq())) -# print("目前的飛行模式 : {}".format(sss)) -# show_time2 = time.time() - -# # if (time.time() - show_time) >= 2: -# # show_time = time.time() -# # for sysid in analyzer.mavlink_systems: -# # for compid in analyzer.mavlink_systems[sysid].components: -# # print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count)) -# # analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid) -# # print("===================") - - - -# analyzer.destroy_node() -# rclpy.shutdown() - - -# # 結束程式 退出所有 thread -# mavlink_object_in.stop() -# mavlink_object_in.thread.join() -# mavlink_socket_in.close() -# mavlink_object_out.stop() -# mavlink_object_out.thread.join() -# mavlink_socket_out.close() -# analyzer.stop() -# analyzer.thread.join() - - -# print('<=== End of Program') - -# elif test_item == 51: - -# # 晉凱的測試項目 -# print('===> Start of Program .Test ', test_item) -# rclpy.init() # 注意要初始化 rclpy 才能使用 node - -# # 啟動 mavlink_bridge -# analyzer = mo.mavlink_bridge() -# # 關於 Node 的初始化 -# show_time = time.time() -# analyzer._init_node() # 初始化 node -# print('初始化 node 完成 耗時 : ',time.time() - show_time) - -# # 創建通道 -# connection_string="udp:127.0.0.1:15551" -# mavlink_socket3 = mavutil.mavlink_connection(connection_string) -# mavlink_object3 = mo.mavlink_object(mavlink_socket3) -# # 設定通道流動 -# mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147] -# mavlink_object3.multiplexingToReturn = [] # -# # mavlink_object3.multiplexingToSwap = [] # -# # 啟動通道 -# mavlink_object3.run() - - - -# print('waiting for mavlink data ...') -# time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息 - -# compid = 1 -# sysid = 1 -# start_time = time.time() -# analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid]) -# end_time = time.time() -# print(f"Execution time for create all topic: {end_time - start_time} seconds") - -# print("start emit info") - -# start_time = time.time() -# show_time = time.time() -# while time.time() - start_time < running_time: -# try: -# # rclpy.spin(analyzer) -# analyzer.emit_info() # 這邊是測試 node 的運行 -# time.sleep(0.5) -# except KeyboardInterrupt: -# break - -# analyzer.destroy_node() -# rclpy.shutdown() - -# # 結束程式 退出所有 thread -# mavlink_object3.stop() -# mavlink_object3.thread.join() -# analyzer.stop() -# analyzer.thread.join() - -# mavlink_socket3.close() -# print('<=== End of Program') diff --git a/src/fc_network_adapter/tests/example_mavlink_client.py b/src/fc_network_adapter/tests/example_mavlink_client.py new file mode 100644 index 0000000..9f59c33 --- /dev/null +++ b/src/fc_network_adapter/tests/example_mavlink_client.py @@ -0,0 +1,180 @@ +""" +MavlinkCommandService 使用範例 + +展示如何從其他 ROS2 節點調用 MAVLink 指令服務 +""" + +import rclpy +from rclpy.node import Node +from std_srvs.srv import Trigger +import time + + +class MavlinkClientExample(Node): + """ + 範例：MAVLink Service Client + + 這個節點展示如何調用 MavlinkCommandService 提供的服務 + """ + + def __init__(self): + super().__init__('mavlink_client_example') + + # 創建 service client + self.client = self.create_client( + Trigger, + '/mavlink/send_command_long' + ) + + # 等待服務可用 + self.get_logger().info('等待 service 可用...') + self.client.wait_for_service() + self.get_logger().info('Service 已連接！') + + def send_arm_command(self): + """ + 範例：發送 ARM 指令 + + 實際使用時應該發送： + - message_id = 76 (COMMAND_LONG) + - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM) + - param1 = 1 (ARM) + """ + self.get_logger().info('發送 ARM 指令...') + + request = Trigger.Request() + # TODO: 實際使用時應該是自定義的 SendCommandLong.Request + # request.target_sysid = 1 + # request.target_compid = 1 + # request.command = 400 # MAV_CMD_COMPONENT_ARM_DISARM + # request.param1 = 1.0 # 1 = ARM, 0 = DISARM + # request.param2 = 0.0 + # ... param3-7 + # request.timeout = 3.0 + + # 異步調用 + future = self.client.call_async(request) + + # 等待結果 + rclpy.spin_until_future_complete(self, future) + + if future.done(): + response = future.result() + if response.success: + self.get_logger().info('✓ ARM 指令發送成功') + else: + self.get_logger().error(f'✗ ARM 指令失敗: {response.message}') + else: + self.get_logger().error('✗ Service 調用超時') + + +def main(): + """主函數""" + rclpy.init() + + # 創建客戶端節點 + client = MavlinkClientExample() + + # 發送指令 + client.send_arm_command() + + # 清理 + client.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + + +""" +═══════════════════════════════════════════════════════════════════════════ +進階使用範例：高層應用程式 + +假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具： + +```python +class MissionController(Node): + def __init__(self): + super().__init__('mission_controller') + + # 創建各種 service clients + self.client_arm = self.create_client( + SendCommandLong, '/mavlink/send_command_long') + self.client_mode = self.create_client( + SendCommandLong, '/mavlink/send_command_long') + self.client_takeoff = self.create_client( + SendCommandLong, '/mavlink/send_command_long') + self.client_goto = self.create_client( + SendCommandInt, '/mavlink/send_command_int') + + def arm_vehicle(self, sysid=1): + \"\"\"解鎖載具\"\"\" + request = SendCommandLong.Request() + request.target_sysid = sysid + request.target_compid = 1 + request.command = 400 # MAV_CMD_COMPONENT_ARM_DISARM + request.param1 = 1.0 # ARM + + future = self.client_arm.call_async(request) + # 處理回應... + + def set_mode(self, sysid=1, mode='GUIDED'): + \"\"\"設置飛行模式\"\"\" + # 實現模式切換邏輯... + pass + + def takeoff(self, sysid=1, altitude=10.0): + \"\"\"起飛\"\"\" + request = SendCommandLong.Request() + request.target_sysid = sysid + request.target_compid = 1 + request.command = 22 # MAV_CMD_NAV_TAKEOFF + request.param7 = altitude + + future = self.client_takeoff.call_async(request) + # 處理回應... + + def goto_position(self, sysid=1, lat=0, lon=0, alt=10): + \"\"\"前往指定位置\"\"\" + request = SendCommandInt.Request() + request.target_sysid = sysid + request.target_compid = 1 + request.command = 192 # MAV_CMD_DO_REPOSITION + request.x = int(lat * 1e7) + request.y = int(lon * 1e7) + request.z = alt + + future = self.client_goto.call_async(request) + # 處理回應... + + def execute_mission(self): + \"\"\"執行完整任務\"\"\" + # 1. 解鎖 + self.arm_vehicle() + time.sleep(1) + + # 2. 切換到 GUIDED 模式 + self.set_mode(mode='GUIDED') + time.sleep(1) + + # 3. 起飛到 10 公尺 + self.takeoff(altitude=10.0) + time.sleep(10) + + # 4. 前往目標點 + self.goto_position(lat=25.033, lon=121.565, alt=10) + time.sleep(30) + + # 5. 返回並降落 + # ... +``` + +這樣的設計讓高層應用可以： +1. 完全不需要知道 MAVLink 協議細節 +2. 通過 ROS2 service 與載具互動 +3. 模組化開發不同功能 +4. 易於測試和維護 + +═══════════════════════════════════════════════════════════════════════════ +""" diff --git a/src/fc_network_apps/__init__.py b/src/fc_network_apps/__init__.py new file mode 100644 index 0000000..d619718 --- /dev/null +++ b/src/fc_network_apps/__init__.py @@ -0,0 +1,14 @@ +from .longCommand import CommandLongClient, ChangeModeResult +from .changeMode import change_mode +from .arm_disarm import arm_disarm +from .takeoff import takeoff +from .land import land + +__all__ = [ + "CommandLongClient", + "ChangeModeResult", + "change_mode", + "arm_disarm", + "takeoff", + "land", +] diff --git a/src/fc_network_apps/arm_disarm.py b/src/fc_network_apps/arm_disarm.py new file mode 100644 index 0000000..21f8386 --- /dev/null +++ b/src/fc_network_apps/arm_disarm.py @@ -0,0 +1,93 @@ +"""Simple wrapper for arm/disarm via fc_network ROS2 service (MAV_CMD_COMPONENT_ARM_DISARM).""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import rclpy +from rclpy.node import Node + +from fc_interfaces.srv import MavCommandLong + +COMMAND_COMPONENT_ARM_DISARM = 400 +DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" +DEFAULT_TIMEOUT_SEC = 2.0 + + +@dataclass +class ArmDisarmResult: + success: bool + message: str + ack_result: int + + +def arm_disarm( + *, + target_sysid: int, + arm: bool, + target_compid: int = 0, + confirmation: int = 0, + param2: float = 0.0, + timeout_sec: float = DEFAULT_TIMEOUT_SEC, + service_name: str = DEFAULT_SERVICE_NAME, +) -> ArmDisarmResult: + """One-shot MAV_CMD_COMPONENT_ARM_DISARM (400) wrapper. + + param1: 1.0 to arm, 0.0 to disarm. + param2: usually 0. Some stacks use 21196 for force-arm (ArduPilot); pass via param2 if needed. + """ + rclpy.init(args=None) + node: Optional[Node] = None + try: + node = Node("fc_arm_disarm_client_once") + client = node.create_client(MavCommandLong, service_name) + + if not client.wait_for_service(timeout_sec=timeout_sec): + return ArmDisarmResult( + success=False, + message=f"Service not available: {service_name}", + ack_result=-1, + ) + + req = MavCommandLong.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.command = COMMAND_COMPONENT_ARM_DISARM + req.confirmation = confirmation + req.param1 = 1.0 if arm else 0.0 + req.param2 = float(param2) + req.param3 = 0.0 + req.param4 = 0.0 + req.param5 = 0.0 + req.param6 = 0.0 + req.param7 = 0.0 + req.timeout_sec = float(timeout_sec) + + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0) + if not future.done() or future.result() is None: + return ArmDisarmResult( + success=False, + message="Service call timeout or no response.", + ack_result=-1, + ) + + response = future.result() + return ArmDisarmResult( + success=response.success, + message=response.message, + ack_result=response.ack_result, + ) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + r = arm_disarm(target_sysid=3, arm=True) + print( + f"arm_disarm success={r.success}, " + f"ack_result={r.ack_result}, message='{r.message}'" + ) diff --git a/src/fc_network_apps/changeMode.py b/src/fc_network_apps/changeMode.py new file mode 100644 index 0000000..aa98230 --- /dev/null +++ b/src/fc_network_apps/changeMode.py @@ -0,0 +1,99 @@ +"""Simple wrapper for mode change via fc_network ROS2 service. + +Reference CLI command: +ros2 service call /fc_network/vehicle/send_command_long \ + fc_interfaces/srv/MavCommandLong \ + "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, \ + param1: 1, param2: 4, param3: 0, param4: 0, param5: 0, param6: 0, \ + param7: 0, timeout_sec: 2}" +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import rclpy +from rclpy.node import Node + +from fc_interfaces.srv import MavCommandLong + +COMMAND_DO_SET_MODE = 176 +DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" +DEFAULT_TIMEOUT_SEC = 2.0 + +@dataclass +class ChangeModeResult: + """Return value for mode change requests.""" + + success: bool + message: str + ack_result: int + +def change_mode( + *, + target_sysid: int, + custom_mode: float, + target_compid: int = 0, + base_mode: float = 1.0, + confirmation: int = 0, + timeout_sec: float = DEFAULT_TIMEOUT_SEC, + service_name: str = DEFAULT_SERVICE_NAME, +) -> ChangeModeResult: + + """One-shot helper for collaborators who want minimal code.""" + rclpy.init(args=None) + node: Optional[Node] = None + try: + node = Node("fc_change_mode_client_once") + client = node.create_client(MavCommandLong, service_name) + + if not client.wait_for_service(timeout_sec=timeout_sec): + return ChangeModeResult( + success=False, + message=f"Service not available: {service_name}", + ack_result=-1, + ) + + req = MavCommandLong.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.command = COMMAND_DO_SET_MODE + req.confirmation = confirmation + req.param1 = float(base_mode) + req.param2 = float(custom_mode) + req.param3 = 0.0 + req.param4 = 0.0 + req.param5 = 0.0 + req.param6 = 0.0 + req.param7 = 0.0 + req.timeout_sec = float(timeout_sec) + + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0) + if not future.done() or future.result() is None: + return ChangeModeResult( + success=False, + message="Service call timeout or no response.", + ack_result=-1, + ) + + response = future.result() + return ChangeModeResult( + success=response.success, + message=response.message, + ack_result=response.ack_result, + ) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + # Example values are aligned with your terminal command. + result = change_mode(target_sysid=3, custom_mode=4) + print( + f"change_mode success={result.success}, " + f"ack_result={result.ack_result}, message='{result.message}'" + ) \ No newline at end of file diff --git a/src/fc_network_apps/land.py b/src/fc_network_apps/land.py new file mode 100644 index 0000000..d21f381 --- /dev/null +++ b/src/fc_network_apps/land.py @@ -0,0 +1,90 @@ +"""Simple wrapper for land via fc_network ROS2 service.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import rclpy +from rclpy.node import Node + +from fc_interfaces.srv import MavCommandLong + +COMMAND_NAV_LAND = 21 +DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" +DEFAULT_TIMEOUT_SEC = 2.0 + + +@dataclass +class LandResult: + success: bool + message: str + ack_result: int + + +def land( + *, + target_sysid: int, + target_compid: int = 0, + yaw_deg: float = 0.0, + latitude: Optional[float] = None, + longitude: Optional[float] = None, + altitude_m: float = 0.0, + timeout_sec: float = DEFAULT_TIMEOUT_SEC, + service_name: str = DEFAULT_SERVICE_NAME, +) -> LandResult: + """One-shot MAV_CMD_NAV_LAND wrapper.""" + rclpy.init(args=None) + node: Optional[Node] = None + try: + node = Node("fc_land_client_once") + client = node.create_client(MavCommandLong, service_name) + + if not client.wait_for_service(timeout_sec=timeout_sec): + return LandResult( + success=False, + message=f"Service not available: {service_name}", + ack_result=-1, + ) + + req = MavCommandLong.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.command = COMMAND_NAV_LAND + req.confirmation = 0 + req.param1 = 0.0 + req.param2 = 0.0 + req.param3 = 0.0 + req.param4 = float(yaw_deg) + req.param5 = float(latitude) if latitude is not None else 0.0 + req.param6 = float(longitude) if longitude is not None else 0.0 + req.param7 = float(altitude_m) + req.timeout_sec = float(timeout_sec) + + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0) + if not future.done() or future.result() is None: + return LandResult( + success=False, + message="Service call timeout or no response.", + ack_result=-1, + ) + + response = future.result() + return LandResult( + success=response.success, + message=response.message, + ack_result=response.ack_result, + ) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + result = land(target_sysid=3) + print( + f"land success={result.success}, " + f"ack_result={result.ack_result}, message='{result.message}'" + ) diff --git a/src/fc_network_apps/longCommand.py b/src/fc_network_apps/longCommand.py new file mode 100644 index 0000000..6911c7c --- /dev/null +++ b/src/fc_network_apps/longCommand.py @@ -0,0 +1,75 @@ + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import rclpy +from rclpy.node import Node + +from fc_interfaces.srv import MavCommandLong + +COMMAND_DO_SET_MODE = 176 +DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" +DEFAULT_TIMEOUT_SEC = 2.0 + + +@dataclass +class ChangeModeResult: + """Return value for mode change requests.""" + + success: bool + message: str + ack_result: int + + +class CommandLongClient(Node): + """Small ROS2 client dedicated to change flight mode.""" + + def __init__(self, service_name: str = DEFAULT_SERVICE_NAME) -> None: + rclpy.init() + super().__init__("fc_change_mode_client") + self._client = self.create_client(MavCommandLong, service_name) + + def wait_for_service(self, timeout_sec: float = 3.0) -> bool: + return self._client.wait_for_service(timeout_sec=timeout_sec) + + def change_mode( + self, + *, + target_sysid: int, + custom_mode: float, + target_compid: int = 0, + base_mode: float = 1.0, + confirmation: int = 0, + timeout_sec: float = DEFAULT_TIMEOUT_SEC ) -> ChangeModeResult: + + req = MavCommandLong.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.command = COMMAND_DO_SET_MODE + req.confirmation = confirmation + req.param1 = float(base_mode) + req.param2 = float(custom_mode) + req.param3 = 0.0 + req.param4 = 0.0 + req.param5 = 0.0 + req.param6 = 0.0 + req.param7 = 0.0 + req.timeout_sec = float(timeout_sec) + + future = self._client.call_async(req) + rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec + 1.0) + if not future.done() or future.result() is None: + return ChangeModeResult( + success=False, + message="Service call timeout or no response.", + ack_result=-1, + ) + + response = future.result() + return ChangeModeResult( + success=response.success, + message=response.message, + ack_result=response.ack_result, + ) \ No newline at end of file diff --git a/src/fc_network_apps/takeoff.py b/src/fc_network_apps/takeoff.py new file mode 100644 index 0000000..47754b7 --- /dev/null +++ b/src/fc_network_apps/takeoff.py @@ -0,0 +1,91 @@ +"""Simple wrapper for takeoff via fc_network ROS2 service.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional + +import rclpy +from rclpy.node import Node + +from fc_interfaces.srv import MavCommandLong + +COMMAND_NAV_TAKEOFF = 22 +DEFAULT_SERVICE_NAME = "/fc_network/vehicle/send_command_long" +DEFAULT_TIMEOUT_SEC = 2.0 + + +@dataclass +class TakeoffResult: + success: bool + message: str + ack_result: int + + +def takeoff( + *, + target_sysid: int, + altitude_m: float, + target_compid: int = 0, + min_pitch_deg: float = 0.0, + yaw_deg: float = 0.0, + latitude: Optional[float] = None, + longitude: Optional[float] = None, + timeout_sec: float = DEFAULT_TIMEOUT_SEC, + service_name: str = DEFAULT_SERVICE_NAME, +) -> TakeoffResult: + """One-shot MAV_CMD_NAV_TAKEOFF wrapper.""" + rclpy.init(args=None) + node: Optional[Node] = None + try: + node = Node("fc_takeoff_client_once") + client = node.create_client(MavCommandLong, service_name) + + if not client.wait_for_service(timeout_sec=timeout_sec): + return TakeoffResult( + success=False, + message=f"Service not available: {service_name}", + ack_result=-1, + ) + + req = MavCommandLong.Request() + req.target_sysid = target_sysid + req.target_compid = target_compid + req.command = COMMAND_NAV_TAKEOFF + req.confirmation = 0 + req.param1 = float(min_pitch_deg) + req.param2 = 0.0 + req.param3 = 0.0 + req.param4 = float(yaw_deg) + req.param5 = float(latitude) if latitude is not None else 0.0 + req.param6 = float(longitude) if longitude is not None else 0.0 + req.param7 = float(altitude_m) + req.timeout_sec = float(timeout_sec) + + future = client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout_sec + 1.0) + if not future.done() or future.result() is None: + return TakeoffResult( + success=False, + message="Service call timeout or no response.", + ack_result=-1, + ) + + response = future.result() + return TakeoffResult( + success=response.success, + message=response.message, + ack_result=response.ack_result, + ) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + result = takeoff(target_sysid=3, altitude_m=10.0) + print( + f"takeoff success={result.success}, " + f"ack_result={result.ack_result}, message='{result.message}'" + ) diff --git a/src/someotherpkg/src/example2_change_mode.py b/src/someotherpkg/src/example2_change_mode.py new file mode 100644 index 0000000..37ef478 --- /dev/null +++ b/src/someotherpkg/src/example2_change_mode.py @@ -0,0 +1,55 @@ + + +from fc_network_apps import CommandLongClient +import time + +def main(): + # Equivalent to: + # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4 + commandAPI = CommandLongClient() + result = commandAPI.change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=4.0, + timeout_sec=2.0, + ) + + print("=== change mode result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + time.sleep(1) + + result = commandAPI.change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=3.0, + timeout_sec=2.0, + ) + + print("=== change mode result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + time.sleep(1) + + result = commandAPI.change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=5.0, + timeout_sec=2.0, + ) + + print("=== change mode result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/someotherpkg/src/example_arm_disarm.py b/src/someotherpkg/src/example_arm_disarm.py new file mode 100644 index 0000000..8b55a27 --- /dev/null +++ b/src/someotherpkg/src/example_arm_disarm.py @@ -0,0 +1,30 @@ +"""Usage example for arm/disarm helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_arm_disarm +""" + +from fc_network_apps import arm_disarm + + +def main() -> None: + # MAV_CMD_COMPONENT_ARM_DISARM (command=400) + # param1: 1 = arm, 0 = disarm + result = arm_disarm( + target_sysid=3, + target_compid=0, + arm=True, + timeout_sec=2.0, + ) + + print("=== arm result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + # To disarm instead: + # result = arm_disarm(target_sysid=3, target_compid=0, arm=False, timeout_sec=2.0) + + +if __name__ == "__main__": + main() diff --git a/src/someotherpkg/src/example_change_mode.py b/src/someotherpkg/src/example_change_mode.py new file mode 100644 index 0000000..108244a --- /dev/null +++ b/src/someotherpkg/src/example_change_mode.py @@ -0,0 +1,29 @@ +"""Usage example for collaborators. + +Run after sourcing your ROS2 workspace: + source install/local_setup.bash + python src/fc_network_apps/example_change_mode.py +""" + +from fc_network_apps import change_mode + + +def main() -> None: + # Equivalent to: + # ros2 service call /fc_network/vehicle/send_command_long ... param1:1 param2:4 + result = change_mode( + target_sysid=3, + target_compid=0, + base_mode=1.0, + custom_mode=4.0, + timeout_sec=2.0, + ) + + print("=== change mode result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + +if __name__ == "__main__": + main() diff --git a/src/someotherpkg/src/example_land.py b/src/someotherpkg/src/example_land.py new file mode 100644 index 0000000..460ca5a --- /dev/null +++ b/src/someotherpkg/src/example_land.py @@ -0,0 +1,27 @@ +"""Usage example for land helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_land +""" + +from fc_network_apps import land + + +def main() -> None: + # MAV_CMD_NAV_LAND (command=21) + # This example asks vehicle sysid=3 to land. + result = land( + target_sysid=3, + target_compid=0, + yaw_deg=0.0, + timeout_sec=2.0, + ) + + print("=== land result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + +if __name__ == "__main__": + main() diff --git a/src/someotherpkg/src/example_takeoff.py b/src/someotherpkg/src/example_takeoff.py new file mode 100644 index 0000000..3d0fa54 --- /dev/null +++ b/src/someotherpkg/src/example_takeoff.py @@ -0,0 +1,28 @@ +"""Usage example for takeoff helper. + +Run from repo root with module mode: + python -m someotherpkg.src.example_takeoff +""" + +from fc_network_apps import takeoff + + +def main() -> None: + # MAV_CMD_NAV_TAKEOFF (command=22) + # This example asks vehicle sysid=3 to take off to 10 meters. + result = takeoff( + target_sysid=3, + target_compid=0, + altitude_m=10.0, + yaw_deg=0.0, + timeout_sec=2.0, + ) + + print("=== takeoff result ===") + print(f"success : {result.success}") + print(f"ack_result: {result.ack_result}") + print(f"message : {result.message}") + + +if __name__ == "__main__": + main() diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt new file mode 100644 index 0000000..b4d7dbc --- /dev/null +++ b/src/unitdev02/unitdev02/devnote.txt @@ -0,0 +1,48 @@ +備選需要的功能 + - serail 對於 telemetry 的支援 + - serial_manager.serial_object.transport 這些變數可能不需要 + +不用動 + - mavlink_object 的 send_message 確認一下 mavlink_bridge 的 _send_to_socket 是不是應該做成 async + - 不同 socket 上面有重複的 sysid 分開儲存 (不做 不允許sysid重複) + +這一步 + 研究 ros2 service + +下一步 + +下下一步 + + +後面 + rssi 資訊提取s + + + +自己的常用指令 +python -m fc_network_adapter.tests.test_vehicleStatusPublisher +python -m fc_network_adapter.tests.test_ringBuffer +python -m fc_network_adapter.fc_network_adapter.mainOrchestrator + +python -m someotherpkg.src.example_change_mode + +ros2 topic list +ros2 topic echo + + +/home/picars/ardupilot/build/sitl/bin/arducopter -S --model + --speedup 1 --slave 0 --defaults /home/picars/ardupilot/Tools/autotest/default_params/copter.parm --sim-address=127.0.0.1 -I3 --sysid 7 + +mavproxy.py --master=tcp:127.0.0.1:5790 --out=udp:127.0.0.1:14560 + + +ros2 service call /mavlink/add_two_ints example_interfaces/srv/AddTwoInts "{a: 5,b: 8}" +ros2 service call /fc_network/vehicle/mav_ping fc_interfaces/srv/MavPing "{target_sysid: 3, target_compid: 0, ping_seq: 1}" + +ros2 service call /fc_network/vehicle/send_command_long fc_interfaces/srv/MavCommandLong "{target_sysid: 3, target_compid: 0, command: 176, confirmation: 0, param1: 1, param2: 4, param3: 0,param4: 0,param5: 0,param6: 0,param7: 0, timeout_sec: 2}" + +MAV_CMD_DO_SET_MODE (176) + + +sudo tcpdump -i lo 'udp dst port 14561' -X +sudo tcpdump -i lo 'udp dst port 14550' -X -vv +sudo tcpdump -i lo -X udp port 14550 \ No newline at end of file diff --git a/src/unitdev02/unitdev02/sslChech.sh b/src/unitdev02/unitdev02/sslChech.sh new file mode 100644 index 0000000..9f38904 --- /dev/null +++ b/src/unitdev02/unitdev02/sslChech.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +# 網站清單 +DOMAINS=("google.com" "smarter.nchu.edu.tw") + +echo "網站 SSL 憑證剩餘天數：" +echo "---------------------------" + +for domain in "${DOMAINS[@]}"; do + end_date=$(echo | openssl s_client -servername "$domain" -connect "$domain:443" 2>/dev/null | + openssl x509 -noout -enddate | cut -d= -f2) + + end_timestamp=$(date -d "$end_date" +%s) + now_timestamp=$(date +%s) + + remaining_days=$(( (end_timestamp - now_timestamp) / 86400 )) + + if [ $remaining_days -lt 0 ]; then + status="已過期 ❌" + elif [ $remaining_days -lt 15 ]; then + status="即將到期 ⚠️" + else + status="正常 ✅" + fi + + printf "%-20s 到期日：%-25s 剩餘天數：%3d 天 %s\n" "$domain" "$end_date" "$remaining_days" "$status" +done