yyb_branch
parent
20b306c3a8
commit
0fa4bc2d3d
@ -0,0 +1,5 @@
|
||||
from .mavlink_handler import MavlinkHandler
|
||||
from .database_handler import DatabaseHandler
|
||||
from .message_queue import MessageQueue
|
||||
|
||||
__all__ = ['MavlinkHandler', 'DatabaseHandler', 'MessageQueue']
|
||||
@ -0,0 +1,150 @@
|
||||
import threading
|
||||
import time
|
||||
from typing import Dict, Any, Optional
|
||||
from .mavlink_handler import MavlinkHandler
|
||||
from .database_handler import DatabaseHandler
|
||||
from .message_queue import MessageQueue
|
||||
|
||||
class CommunicationManager:
|
||||
def __init__(self, mavlink_connection: str, db_config: Dict[str, str]):
|
||||
self.mavlink = MavlinkHandler(mavlink_connection)
|
||||
self.database = DatabaseHandler(
|
||||
host=db_config['host'],
|
||||
user=db_config['user'],
|
||||
password=db_config['password'],
|
||||
database=db_config['database']
|
||||
)
|
||||
self.running = False
|
||||
self.thread = None
|
||||
self.message_queue = MessageQueue()
|
||||
|
||||
def start(self) -> bool:
|
||||
"""启动通信管理器"""
|
||||
if not self.database.connect():
|
||||
return False
|
||||
|
||||
# 创建数据库表
|
||||
self.database.create_tables()
|
||||
|
||||
# 启动MAVLink连接
|
||||
if not self.mavlink.connect():
|
||||
return False
|
||||
|
||||
# 注册MAVLink消息回调
|
||||
self.mavlink.register_callback('HEARTBEAT', self._handle_heartbeat)
|
||||
self.mavlink.register_callback('GLOBAL_POSITION_INT', self._handle_position)
|
||||
self.mavlink.register_callback('SYS_STATUS', self._handle_system_status)
|
||||
|
||||
# 启动MAVLink消息接收
|
||||
self.mavlink.start()
|
||||
|
||||
# 启动消息处理线程
|
||||
self.running = True
|
||||
self.thread = threading.Thread(target=self._process_messages)
|
||||
self.thread.daemon = True
|
||||
self.thread.start()
|
||||
|
||||
return True
|
||||
|
||||
def stop(self):
|
||||
"""停止通信管理器"""
|
||||
self.running = False
|
||||
if self.thread:
|
||||
self.thread.join()
|
||||
self.mavlink.stop()
|
||||
self.database.disconnect()
|
||||
|
||||
def _process_messages(self):
|
||||
"""处理消息队列中的消息"""
|
||||
while self.running:
|
||||
try:
|
||||
msg = self.mavlink.get_message_queue().get(timeout=1.0)
|
||||
if msg:
|
||||
self._handle_message(msg)
|
||||
except Exception as e:
|
||||
print(f"消息处理错误: {str(e)}")
|
||||
time.sleep(1)
|
||||
|
||||
def _handle_message(self, msg: Dict[str, Any]):
|
||||
"""处理接收到的消息"""
|
||||
try:
|
||||
msg_type = msg['message'].get_type()
|
||||
msg_data = msg['message'].to_dict()
|
||||
|
||||
# 保存消息到数据库
|
||||
self.database.save_mavlink_message(msg_type, msg_data)
|
||||
|
||||
# 根据消息类型更新无人机状态
|
||||
if msg_type == 'GLOBAL_POSITION_INT':
|
||||
self._update_drone_position(msg_data)
|
||||
elif msg_type == 'SYS_STATUS':
|
||||
self._update_drone_status(msg_data)
|
||||
|
||||
except Exception as e:
|
||||
print(f"消息处理失败: {str(e)}")
|
||||
|
||||
def _handle_heartbeat(self, msg):
|
||||
"""处理心跳消息"""
|
||||
self.message_queue.put({
|
||||
'type': 'HEARTBEAT',
|
||||
'data': msg.to_dict()
|
||||
})
|
||||
|
||||
def _handle_position(self, msg):
|
||||
"""处理位置消息"""
|
||||
self.message_queue.put({
|
||||
'type': 'POSITION',
|
||||
'data': msg.to_dict()
|
||||
})
|
||||
|
||||
def _handle_system_status(self, msg):
|
||||
"""处理系统状态消息"""
|
||||
self.message_queue.put({
|
||||
'type': 'SYSTEM_STATUS',
|
||||
'data': msg.to_dict()
|
||||
})
|
||||
|
||||
def _update_drone_position(self, position_data: Dict[str, Any]):
|
||||
"""更新无人机位置信息"""
|
||||
query = """
|
||||
INSERT INTO drone_status
|
||||
(drone_id, latitude, longitude, altitude, timestamp)
|
||||
VALUES (%s, %s, %s, %s, %s)
|
||||
"""
|
||||
params = (
|
||||
position_data.get('sysid', 'unknown'),
|
||||
position_data.get('lat', 0) / 1e7, # 转换为度
|
||||
position_data.get('lon', 0) / 1e7, # 转换为度
|
||||
position_data.get('alt', 0) / 1000, # 转换为米
|
||||
datetime.now()
|
||||
)
|
||||
self.database.execute_update(query, params)
|
||||
|
||||
def _update_drone_status(self, status_data: Dict[str, Any]):
|
||||
"""更新无人机状态信息"""
|
||||
query = """
|
||||
UPDATE drone_status
|
||||
SET battery_level = %s, status = %s
|
||||
WHERE drone_id = %s
|
||||
ORDER BY timestamp DESC LIMIT 1
|
||||
"""
|
||||
params = (
|
||||
status_data.get('battery_remaining', 0),
|
||||
status_data.get('system_status', 'UNKNOWN'),
|
||||
status_data.get('sysid', 'unknown')
|
||||
)
|
||||
self.database.execute_update(query, params)
|
||||
|
||||
def get_drone_status(self, drone_id: str) -> Optional[Dict[str, Any]]:
|
||||
"""获取指定无人机的状态"""
|
||||
query = """
|
||||
SELECT * FROM drone_status
|
||||
WHERE drone_id = %s
|
||||
ORDER BY timestamp DESC LIMIT 1
|
||||
"""
|
||||
result = self.database.execute_query(query, (drone_id,))
|
||||
return result[0] if result else None
|
||||
|
||||
def get_message_queue(self) -> MessageQueue:
|
||||
"""获取消息队列"""
|
||||
return self.message_queue
|
||||
@ -0,0 +1,129 @@
|
||||
import mysql.connector
|
||||
from mysql.connector import Error
|
||||
from typing import Dict, List, Any, Optional
|
||||
import json
|
||||
from datetime import datetime
|
||||
|
||||
class DatabaseHandler:
|
||||
#替换数据库账号密码
|
||||
def __init__(self, host: str, user: str, password: str, database: str):
|
||||
self.host = host
|
||||
self.user = user
|
||||
self.password = password
|
||||
self.database = database
|
||||
self.connection = None
|
||||
self.cursor = None
|
||||
|
||||
def connect(self) -> bool:
|
||||
"""连接到MySQL数据库"""
|
||||
try:
|
||||
self.connection = mysql.connector.connect(
|
||||
host=self.host,
|
||||
user=self.user,
|
||||
password=self.password,
|
||||
database=self.database
|
||||
)
|
||||
self.cursor = self.connection.cursor(dictionary=True)
|
||||
print(f"数据库连接成功: {self.database}")
|
||||
return True
|
||||
except Error as e:
|
||||
print(f"数据库连接失败: {str(e)}")
|
||||
return False
|
||||
|
||||
def disconnect(self):
|
||||
"""断开数据库连接"""
|
||||
if self.cursor:
|
||||
self.cursor.close()
|
||||
if self.connection:
|
||||
self.connection.close()
|
||||
|
||||
def execute_query(self, query: str, params: Optional[tuple] = None) -> List[Dict]:
|
||||
"""执行查询并返回结果"""
|
||||
try:
|
||||
self.cursor.execute(query, params)
|
||||
return self.cursor.fetchall()
|
||||
except Error as e:
|
||||
print(f"查询执行失败: {str(e)}")
|
||||
return []
|
||||
|
||||
def execute_update(self, query: str, params: Optional[tuple] = None) -> bool:
|
||||
"""执行更新操作"""
|
||||
try:
|
||||
self.cursor.execute(query, params)
|
||||
self.connection.commit()
|
||||
return True
|
||||
except Error as e:
|
||||
print(f"更新执行失败: {str(e)}")
|
||||
self.connection.rollback()
|
||||
return False
|
||||
|
||||
def save_mavlink_message(self, msg_type: str, msg_data: Dict) -> bool:
|
||||
"""保存MAVLink消息到数据库"""
|
||||
query = """
|
||||
INSERT INTO mavlink_messages
|
||||
(message_type, message_data, timestamp)
|
||||
VALUES (%s, %s, %s)
|
||||
"""
|
||||
params = (
|
||||
msg_type,
|
||||
json.dumps(msg_data),
|
||||
datetime.now()
|
||||
)
|
||||
return self.execute_update(query, params)
|
||||
|
||||
def get_mavlink_messages(self, msg_type: Optional[str] = None,
|
||||
start_time: Optional[datetime] = None,
|
||||
end_time: Optional[datetime] = None) -> List[Dict]:
|
||||
"""获取MAVLink消息"""
|
||||
query = "SELECT * FROM mavlink_messages WHERE 1=1"
|
||||
params = []
|
||||
|
||||
if msg_type:
|
||||
query += " AND message_type = %s"
|
||||
params.append(msg_type)
|
||||
if start_time:
|
||||
query += " AND timestamp >= %s"
|
||||
params.append(start_time)
|
||||
if end_time:
|
||||
query += " AND timestamp <= %s"
|
||||
params.append(end_time)
|
||||
|
||||
query += " ORDER BY timestamp DESC"
|
||||
return self.execute_query(query, tuple(params) if params else None)
|
||||
|
||||
def create_tables(self):
|
||||
"""创建必要的数据库表"""
|
||||
queries = [
|
||||
"""
|
||||
CREATE TABLE IF NOT EXISTS mavlink_messages (
|
||||
id INT AUTO_INCREMENT PRIMARY KEY,
|
||||
message_type VARCHAR(50) NOT NULL,
|
||||
message_data JSON NOT NULL,
|
||||
timestamp DATETIME NOT NULL,
|
||||
INDEX idx_message_type (message_type),
|
||||
INDEX idx_timestamp (timestamp)
|
||||
)
|
||||
""",
|
||||
"""
|
||||
CREATE TABLE IF NOT EXISTS drone_status (
|
||||
id INT AUTO_INCREMENT PRIMARY KEY,
|
||||
drone_id VARCHAR(50) NOT NULL,
|
||||
latitude FLOAT,
|
||||
longitude FLOAT,
|
||||
altitude FLOAT,
|
||||
battery_level FLOAT,
|
||||
status VARCHAR(50),
|
||||
timestamp DATETIME NOT NULL,
|
||||
INDEX idx_drone_id (drone_id),
|
||||
INDEX idx_timestamp (timestamp)
|
||||
)
|
||||
"""
|
||||
]
|
||||
|
||||
for query in queries:
|
||||
try:
|
||||
self.cursor.execute(query)
|
||||
self.connection.commit()
|
||||
except Error as e:
|
||||
print(f"创建表失败: {str(e)}")
|
||||
self.connection.rollback()
|
||||
@ -0,0 +1,81 @@
|
||||
import pymavlink.mavutil as mavutil
|
||||
import threading
|
||||
import time
|
||||
from typing import Callable, Dict, Any
|
||||
from .message_queue import MessageQueue
|
||||
|
||||
class MavlinkHandler:
|
||||
def __init__(self, connection_string: str = "udp:127.0.0.1:14550"):
|
||||
self.connection_string = connection_string
|
||||
self.connection = None
|
||||
self.message_queue = MessageQueue()
|
||||
self.running = False
|
||||
self.thread = None
|
||||
self.callbacks: Dict[str, Callable] = {}
|
||||
|
||||
def connect(self) -> bool:
|
||||
"""建立MAVLink连接"""
|
||||
try:
|
||||
self.connection = mavutil.mavlink_connection(self.connection_string)#连接无人机的IP地址、蓝牙、串口等
|
||||
self.connection.wait_heartbeat()
|
||||
print(f"MAVLink连接成功: {self.connection_string}")
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"MAVLink连接失败: {str(e)}")
|
||||
return False
|
||||
|
||||
def start(self):
|
||||
"""启动MAVLink消息接收线程"""
|
||||
if not self.connection:
|
||||
if not self.connect():
|
||||
return
|
||||
|
||||
self.running = True
|
||||
self.thread = threading.Thread(target=self._receive_loop)
|
||||
self.thread.daemon = True
|
||||
self.thread.start()
|
||||
|
||||
def stop(self):
|
||||
"""停止MAVLink消息接收"""
|
||||
self.running = False
|
||||
if self.thread:
|
||||
self.thread.join()
|
||||
if self.connection:
|
||||
self.connection.close()
|
||||
|
||||
def _receive_loop(self):
|
||||
"""MAVLink消息接收循环"""
|
||||
while self.running:
|
||||
try:
|
||||
msg = self.connection.recv_match(blocking=True, timeout=1.0)
|
||||
if msg:
|
||||
self._handle_message(msg)
|
||||
except Exception as e:
|
||||
print(f"接收消息错误: {str(e)}")
|
||||
time.sleep(1)
|
||||
|
||||
def _handle_message(self, msg):
|
||||
"""处理接收到的MAVLink消息"""
|
||||
msg_type = msg.get_type()
|
||||
if msg_type in self.callbacks:
|
||||
self.callbacks[msg_type](msg)
|
||||
self.message_queue.put(msg)
|
||||
|
||||
def register_callback(self, msg_type: str, callback: Callable):
|
||||
"""注册消息回调函数"""
|
||||
self.callbacks[msg_type] = callback
|
||||
|
||||
def send_message(self, msg_type: str, **kwargs):
|
||||
"""发送MAVLink消息"""
|
||||
if not self.connection:
|
||||
return False
|
||||
try:
|
||||
self.connection.mav.send(mavutil.mavlink.MAVLink_message_type_map[msg_type](**kwargs))
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"发送消息失败: {str(e)}")
|
||||
return False
|
||||
|
||||
def get_message_queue(self) -> MessageQueue:
|
||||
"""获取消息队列"""
|
||||
return self.message_queue
|
||||
@ -0,0 +1,65 @@
|
||||
import queue
|
||||
import threading
|
||||
from typing import Any, Optional
|
||||
from datetime import datetime
|
||||
|
||||
class MessageQueue:
|
||||
def __init__(self, maxsize: int = 1000):
|
||||
self.queue = queue.Queue(maxsize=maxsize)
|
||||
self.lock = threading.Lock()
|
||||
self.callbacks = []
|
||||
|
||||
def put(self, message: Any):
|
||||
"""将消息放入队列"""
|
||||
with self.lock:
|
||||
try:
|
||||
self.queue.put_nowait({
|
||||
'message': message,
|
||||
'timestamp': datetime.now()
|
||||
})
|
||||
self._notify_callbacks()
|
||||
except queue.Full:
|
||||
print("消息队列已满，丢弃消息")
|
||||
|
||||
def get(self, block: bool = True, timeout: Optional[float] = None) -> Optional[dict]:
|
||||
"""从队列中获取消息"""
|
||||
try:
|
||||
return self.queue.get(block=block, timeout=timeout)
|
||||
except queue.Empty:
|
||||
return None
|
||||
|
||||
def register_callback(self, callback: callable):
|
||||
"""注册消息回调函数"""
|
||||
with self.lock:
|
||||
self.callbacks.append(callback)
|
||||
|
||||
def unregister_callback(self, callback: callable):
|
||||
"""注销消息回调函数"""
|
||||
with self.lock:
|
||||
if callback in self.callbacks:
|
||||
self.callbacks.remove(callback)
|
||||
|
||||
def _notify_callbacks(self):
|
||||
"""通知所有注册的回调函数"""
|
||||
for callback in self.callbacks:
|
||||
try:
|
||||
callback()
|
||||
except Exception as e:
|
||||
print(f"回调函数执行失败: {str(e)}")
|
||||
|
||||
def clear(self):
|
||||
"""清空消息队列"""
|
||||
with self.lock:
|
||||
while not self.queue.empty():
|
||||
try:
|
||||
self.queue.get_nowait()
|
||||
except queue.Empty:
|
||||
break
|
||||
|
||||
def size(self) -> int:
|
||||
"""获取队列大小"""
|
||||
return self.queue.qsize()
|
||||
|
||||
def is_empty(self) -> bool:
|
||||
"""检查队列是否为空"""
|
||||
return self.queue.empty()
|
||||
Loading…
Reference in new issue