vk_mavlink2_Gui/app.py

"""VKFly telemetry monitor. Read-only: never sends arm or flight commands."""
import math
import sys
import time
from collections import Counter
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
    def run(self):
        try:
            with serial.Serial(self.port, self.baud, timeout=0.15) as conn:
                parser=vkfly.MAVLink(None); parser.robust_parsing=True
                self.status.emit(f'已连接 {self.port} · {self.baud} · 只读')
                while self.running:
                    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}')
        finally:
            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
        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.status=QLabel('未连接 · 被动接收，不发送控制指令');bar.addWidget(QLabel('串口'));bar.addWidget(self.ports);bar.addWidget(refresh);bar.addWidget(QLabel('波特率'));bar.addWidget(self.baud);bar.addWidget(self.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.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
        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.button.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()}')
        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':
            v='未知' if m.voltage_battery==65535 else f'{m.voltage_battery/1000:.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=='AUTOPILOT_VERSION':self.labels['sn'].setText(str(m.uid))
        elif name=='VK_COMP_VERSION' and m.comp_id==1:
            val=m.SN; self.labels['sn'].setText(bytes(val).split(b'\0')[0].decode('ascii','replace'))
        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())