vk_mavlink2_Gui/app.py

"""VKFly telemetry monitor. Sends only flight-controller SN requests."""
import math
import sys
import time
from collections import Counter
from queue import Empty, Queue
from PyQt6.QtCore import Qt, QThread, pyqtSignal, QTimer, QRectF
from PyQt6.QtGui import QColor, QPainter, QPen, QFont, QPolygonF
from PyQt6.QtCore import QPointF
from PyQt6.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout, QHBoxLayout,
    QGridLayout, QLabel, QPushButton, QComboBox, QFrame, QProgressBar, QTableWidget,
    QTableWidgetItem, QHeaderView, QSplitter, QTabWidget, QPlainTextEdit)
import serial
import serial.tools.list_ports
from generated import vkfly

DARK = '#0b1322'

class Link(QThread):
    message = pyqtSignal(object)
    status = pyqtSignal(str)
    def __init__(self, port, baud):
        super().__init__(); self.port=port; self.baud=baud; self.running=True
        self.sn_requests=Queue()
    def request_sn(self, target_system):
        self.sn_requests.put(target_system)
    def run(self):
        try:
            with serial.Serial(self.port, self.baud, timeout=0.15, write_timeout=1) as conn:
                parser=vkfly.MAVLink(None); parser.robust_parsing=True
                sender=vkfly.MAVLink(conn, srcSystem=255, srcComponent=vkfly.MAV_COMP_ID_MISSIONPLANNER)
                self.status.emit(f'已连接 {self.port} · {self.baud} · 仅请求 SN')
                while self.running:
                    try:
                        target_system=self.sn_requests.get_nowait()
                    except Empty:
                        pass
                    else:
                        sender.command_long_send(target_system, vkfly.MAV_COMP_ID_AUTOPILOT1,
                            vkfly.MAV_CMD_REQUEST_MESSAGE, 0, vkfly.MAVLINK_MSG_ID_VK_COMP_VERSION,
                            1, 0, 0, 0, 0, 0)
                    data=conn.read(min(conn.in_waiting or 1, 4096))
                    for byte in data:
                        msg=parser.parse_char(bytes((byte,)))
                        if msg is not None and msg.get_type() != 'BAD_DATA': self.message.emit(msg)
        except Exception as exc:
            self.status.emit(f'串口错误：{exc}')
        else:
            self.status.emit('连接已关闭')
    def stop(self):
        self.running=False
        self.wait(1500)

class Attitude(QWidget):
    def __init__(self):
        super().__init__(); self.roll=None; self.pitch=None; self.yaw=None; self.setMinimumSize(250,220)
    def layout_geometry(self):
        # Reserve a separate strip for the attitude readout at every widget size.
        text_height = 48
        available_height = max(0, self.height() - text_height - 12)
        radius = max(0, min(self.width() - 12, available_height) / 2)
        cx = self.width() / 2
        cy = 6 + available_height / 2
        return cx, cy, radius, QRectF(-self.width()/2 + 4, available_height - cy + 10, self.width() - 8, text_height)
    def paintEvent(self, event):
        p=QPainter(self); p.setRenderHint(QPainter.RenderHint.Antialiasing)
        cx, cy, radius, text_rect = self.layout_geometry()
        p.translate(cx,cy); p.setBrush(QColor('#18283b')); p.setPen(QPen(QColor('#40576f'),2)); p.drawEllipse(QRectF(-radius,-radius,2*radius,2*radius))
        if self.roll is not None and self.pitch is not None:
            p.save(); clip=__import__('PyQt6.QtGui',fromlist=['QPainterPath']).QPainterPath(); clip.addEllipse(QRectF(-radius,-radius,2*radius,2*radius)); p.setClipPath(clip)
            p.rotate(-math.degrees(self.roll)); y=max(-radius*1.5,min(radius*1.5,math.degrees(self.pitch)*radius/45))
            p.fillRect(QRectF(-radius*3,-radius*3+y,radius*6,radius*3),QColor('#2476ab'))
            p.fillRect(QRectF(-radius*3,y,radius*6,radius*3),QColor('#866044'))
            p.setPen(QPen(QColor('#dcefff'),2)); p.drawLine(QPointF(-radius*3,y),QPointF(radius*3,y))
            for deg in (-30,-20,-10,10,20,30):
                yy=y-deg*radius/45; p.drawLine(QPointF(-14,yy),QPointF(14,yy))
            p.restore()
        p.setPen(QPen(QColor('#f6d46a'),3)); p.drawLine(QPointF(-55,0),QPointF(-12,0)); p.drawLine(QPointF(12,0),QPointF(55,0)); p.drawEllipse(QRectF(-4,-4,8,8))
        p.setPen(QColor('#e5efff')); p.setFont(QFont('Microsoft YaHei',10)); text='等待姿态数据' if self.roll is None else f'横滚 {math.degrees(self.roll):.1f}°   俯仰 {math.degrees(self.pitch):.1f}°   航向 {math.degrees(self.yaw):.1f}°'
        p.drawText(text_rect, Qt.AlignmentFlag.AlignCenter | Qt.TextFlag.TextWordWrap, text)

class Radar(QWidget):
    def __init__(self): super().__init__(); self.points=[]; self.range=10; self.setMinimumSize(220,210)
    def paintEvent(self,e):
        p=QPainter(self);p.setRenderHint(QPainter.RenderHint.Antialiasing);cx=self.width()/2;cy=self.height()/2;r=min(self.width(),self.height())*.40
        p.translate(cx,cy);p.setPen(QPen(QColor('#35566b'),1))
        for f in (.33,.66,1):p.drawEllipse(QRectF(-r*f,-r*f,2*r*f,2*r*f))
        p.drawLine(QPointF(-r,0),QPointF(r,0));p.drawLine(QPointF(0,-r),QPointF(0,r))
        p.setBrush(QColor('#40d8c0'));p.setPen(Qt.PenStyle.NoPen);p.drawPolygon(QPolygonF([QPointF(0,-10),QPointF(-6,7),QPointF(6,7)]))
        p.setBrush(QColor('#ffbd61'))
        for angle,d in self.points:
            rr=min(d/self.range,1)*r; a=math.radians(angle); p.drawEllipse(QPointF(math.sin(a)*rr,-math.cos(a)*rr),4,4)
        p.setPen(QColor('#adc5d8'));p.drawText(QRectF(-r,-r-20,2*r,20),Qt.AlignmentFlag.AlignCenter,'前方 · 距离量程 10 m')
        if not self.points:p.drawText(QRectF(-r,r+4,2*r,22),Qt.AlignmentFlag.AlignCenter,'等待有效避障数据')

class Monitor(QMainWindow):
    def __init__(self):
        super().__init__();self.setWindowTitle('VK 飞控遥测监控');self.resize(1400,900);self.link=None;self.counts=Counter();self.rows={};self.seen={};self.values={};self.selected=None;self.flight_system=None
        root=QWidget();self.setCentralWidget(root); outer=QVBoxLayout(root)
        bar=QHBoxLayout();self.ports=QComboBox();self.refresh_ports();refresh=QPushButton('刷新串口');refresh.clicked.connect(self.refresh_ports);self.baud=QComboBox();self.baud.addItems(['115200','230400','460800','57600','921600']);self.button=QPushButton('连接');self.button.clicked.connect(self.toggle)
        self.sn_button=QPushButton('重新请求 SN');self.sn_button.setEnabled(False);self.sn_button.clicked.connect(self.request_sn)
        self.status=QLabel('未连接 · 仅请求 SN，不发送控制指令');bar.addWidget(QLabel('串口'));bar.addWidget(self.ports);bar.addWidget(refresh);bar.addWidget(QLabel('波特率'));bar.addWidget(self.baud);bar.addWidget(self.button);bar.addWidget(self.sn_button);bar.addWidget(self.status,1);outer.addLayout(bar)
        splitter=QSplitter(Qt.Orientation.Horizontal);outer.addWidget(splitter,1)
        left=QWidget();ll=QVBoxLayout(left); title=QLabel('飞行态势');title.setObjectName('title');ll.addWidget(title)
        visuals=QHBoxLayout();self.att=Attitude();self.radar=Radar();visuals.addWidget(self.att,1);visuals.addWidget(self.radar,1);ll.addLayout(visuals)
        grid=QGridLayout(); self.labels={}
        for idx,(key,title) in enumerate([('mode','飞行状态'),('location','地理位置'),('altitude','高度'),('velocity','地速 / 爬升'),('gps','卫星 / 定位'),('power','电源'),('ins','惯导状态'),('sn','飞控 SN')]):
            box=QFrame();box.setObjectName('card');v=QVBoxLayout(box);heading=QLabel(title);heading.setObjectName('heading');value=QLabel('等待数据');value.setWordWrap(True);value.setObjectName('value');v.addWidget(heading);v.addWidget(value);grid.addWidget(box,idx//2,idx%2);self.labels[key]=value
        ll.addLayout(grid);ll.addStretch();splitter.addWidget(left)
        right=QWidget();rl=QVBoxLayout(right);heading=QLabel('消息观察器');heading.setObjectName('title');rl.addWidget(heading);rl.addWidget(QLabel('所有成功解码的标准及 VK 自定义消息；点击行查看字段。灰色表示超过 3 秒未更新。'))
        self.table=QTableWidget(0,4);self.table.setHorizontalHeaderLabels(['消息','ID','数量','状态']);self.table.horizontalHeader().setSectionResizeMode(0,QHeaderView.ResizeMode.Stretch);self.table.setSelectionBehavior(QTableWidget.SelectionBehavior.SelectRows);self.table.setEditTriggers(QTableWidget.EditTrigger.NoEditTriggers);self.table.itemSelectionChanged.connect(self.show_details);rl.addWidget(self.table,3)
        self.detail=QPlainTextEdit();self.detail.setReadOnly(True);self.detail.setPlaceholderText('选择消息查看全部字段');rl.addWidget(self.detail,2);splitter.addWidget(right);splitter.setSizes([650,750])
        self.timer=QTimer(self);self.timer.timeout.connect(self.refresh_age);self.timer.start(500)
    def refresh_ports(self):
        old=self.ports.currentText() if hasattr(self,'ports') else '';self.ports.clear();self.ports.addItems([x.device for x in serial.tools.list_ports.comports()]);idx=self.ports.findText(old)
        if idx>=0:self.ports.setCurrentIndex(idx)
    def toggle(self):
        if self.link:
            self.link.stop();self.link=None;self.flight_system=None;self.sn_button.setEnabled(False);self.button.setText('连接');return
        port=self.ports.currentText()
        if not port:self.status.setText('没有可用串口，请刷新');return
        self.counts.clear();self.rows.clear();self.seen.clear();self.values.clear();self.table.setRowCount(0);self.detail.clear();self.selected=None;self.flight_system=None;self.sn_button.setEnabled(False)
        for label in self.labels.values(): label.setText('等待数据')
        self.att.roll=self.att.pitch=self.att.yaw=None;self.att.update();self.radar.points=[];self.radar.update()
        self.link=Link(port,int(self.baud.currentText()));self.link.message.connect(self.receive);self.link.status.connect(self.set_status);self.link.finished.connect(self.finished);self.button.setText('断开');self.link.start()
    def set_status(self,s):self.status.setText(s)
    def finished(self):
        if self.link and not self.link.isRunning():
            self.link=None;self.flight_system=None;self.sn_button.setEnabled(False);self.button.setText('连接')
    def request_sn(self):
        if self.link and self.flight_system is not None:
            self.link.request_sn(self.flight_system)
            self.labels['sn'].setText('已请求，等待飞控回复')
    def receive(self,m):
        name=m.get_type(); self.counts[name]+=1;self.seen[name]=time.monotonic();self.values[name]=m.to_dict()
        if name not in self.rows:
            row=self.table.rowCount();self.table.insertRow(row);self.rows[name]=row
            for col,val in enumerate((name,str(m.get_msgId()),'0','在线')): self.table.setItem(row,col,QTableWidgetItem(val))
        row=self.rows[name];self.table.item(row,2).setText(str(self.counts[name]));self.table.item(row,3).setText('在线')
        if self.selected==name:self.update_detail(name)
        if name=='ATTITUDE':
            self.att.roll=m.roll;self.att.pitch=m.pitch;self.att.yaw=m.yaw;self.att.update()
        elif name=='HEARTBEAT':
            armed=bool(m.base_mode & 128);self.labels['mode'].setText(f'{"已解锁" if armed else "已上锁"}  ·  模式 {m.custom_mode}\n系统 {m.get_srcSystem()} / 组件 {m.get_srcComponent()}')
            if self.link and self.flight_system is None and m.get_srcSystem()>0 and m.get_srcComponent()==vkfly.MAV_COMP_ID_AUTOPILOT1 and m.autopilot != vkfly.MAV_AUTOPILOT_INVALID:
                self.flight_system=m.get_srcSystem();self.sn_button.setEnabled(True);self.request_sn()
        elif name=='GLOBAL_POSITION_INT':
            self.labels['location'].setText(f'{m.lat/1e7:.7f}°, {m.lon/1e7:.7f}°');self.labels['altitude'].setText(f'海拔 {m.alt/1000:.1f} m · 相对 {m.relative_alt/1000:.1f} m');self.labels['velocity'].setText(f'水平 {math.hypot(m.vx,m.vy)/100:.1f} m/s · 上升 {-m.vz/100:.1f} m/s')
        elif name=='GPS2_RAW':self.labels['gps'].setText(f'{m.satellites_visible} 颗 · fix_type {m.fix_type}')
        elif name=='SYS_STATUS':
            # VKFly uses centivolts here, unlike standard MAVLink's millivolts.
            v='未知' if m.voltage_battery==65535 else f'{m.voltage_battery/100:.1f} V'; pct='未知' if m.battery_remaining<0 else f'{m.battery_remaining}%';self.labels['power'].setText(f'{v} · 剩余 {pct}')
        elif name=='VK_BMS_STATUS':self.labels['power'].setText(f'{m.voltage/1000:.1f} V · 剩余 {m.cap_percent}% · 电池 {m.bat_id}')
        elif name=='VKINS_STATUS':self.labels['ins'].setText(f'状态位 0x{m.nav_status:02X} · 定位星 {m.solv_psat_num} · 定向星 {m.solv_hsat_num}')
        elif name=='VK_COMP_VERSION' and m.comp_id==1 and (self.flight_system is None or m.get_srcSystem()==self.flight_system):
            val=bytes(m.SN).split(b'\0')[0].decode('ascii','replace');self.labels['sn'].setText(val or '飞控未提供 SN')
        elif name=='OBSTACLE_DISTANCE':
            # This plot assumes MAV_FRAME_BODY_FRD and the MAVLink angular convention.
            if m.frame != 12:self.radar.points=[]
            else:
                inc=m.increment_f if m.increment_f>0 else m.increment
                self.radar.points=[(m.angle_offset+i*inc,d/100) for i,d in enumerate(m.distances) if inc>0 and d not in (0,65535) and m.min_distance<=d<=m.max_distance]
            self.radar.update()
    def show_details(self):
        row=self.table.currentRow()
        if row>=0 and self.table.item(row,0):self.selected=self.table.item(row,0).text();self.update_detail(self.selected)
    def update_detail(self,name):
        data=self.values.get(name,{});self.detail.setPlainText('\n'.join(f'{k}: {v}' for k,v in data.items()))
    def refresh_age(self):
        now=time.monotonic()
        for name,row in self.rows.items():
            age=now-self.seen[name];item=self.table.item(row,3);item.setText('在线' if age<3 else f'{age:.0f} 秒前');item.setForeground(QColor('#53d6b1' if age<3 else '#8192a5'))
    def closeEvent(self,event):
        if self.link:self.link.stop()
        super().closeEvent(event)

def main():
    app=QApplication(sys.argv);app.setStyle('Fusion');app.setStyleSheet('''QWidget{background:#0b1322;color:#deebf7;font-family:Microsoft YaHei;font-size:13px} #title{font-size:20px;font-weight:bold;color:#f1f8ff} #heading{color:#86a8bd;font-size:12px} #value{font-size:15px;font-weight:bold} #card{background:#142337;border:1px solid #253e53;border-radius:8px;padding:8px} QPushButton,QComboBox{background:#20394d;border:1px solid #385b71;border-radius:5px;padding:7px} QPushButton:hover{background:#28556a} QTableWidget,QPlainTextEdit{background:#101d2c;border:1px solid #253e53} QHeaderView::section{background:#1d3448;padding:5px;border:0}''')
    win=Monitor()
    if '--self-test' in sys.argv:
        m=vkfly.MAVLink(None,srcSystem=1,srcComponent=1)
        win.receive(m.heartbeat_encode(2,3,0,0,4,3))
        win.receive(m.attitude_encode(1,0.2,0.1,0.5,0,0,0))
        app.processEvents()
        return 0 if win.table.rowCount()==2 and abs(win.att.roll-0.2)<1e-6 else 1
    win.show();return app.exec()
if __name__=='__main__':sys.exit(main())