|
|
@@ -0,0 +1,166 @@
|
|
|
+"""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())
|