利用matplotlib和PyQt5绘制姿态球

前言

这个博文可以说是承接前几篇的MPU6050数据读取、姿态解算，这里主要是实现姿态的可视化（也是课程作业要求，爬了）。
鉴于博主喜欢摸鱼的特点，这个实现也是ddl前几天完成的，由于网上没找到具体实现，只好用极坐标这个沙雕方法硬算出来。

将matplotlib嵌入PyQt5

matplotlib提供了在Qt中进行显示的一个backend（应该是？)，这个方法基本上是网上统一这么实现（狗头）。
将matplotlib嵌入PyQt5一般是将其下面的matplotlib.backends.backend_qt5agg中的FigureCanvasQTAgg作为一个自己创建的类（可以理解为画布）(如这里的MyFigure)的父类，然后将自己创建的MyFigure类作为一个Widget放入QMainWindow或其他容器中（如QGroupBox）。
这里的代码是直接根据这篇博文进行修改的。

import matplotlib
matplotlib.use("Qt5Agg")  # 声明使用QT5
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
 
class MyFigure(FigureCanvas):
    def __init__(self, width=5, height=4, dpi=100):
        # 第一步：创建一个创建Figure
        self.fig = Figure(figsize=(width, height), dpi=dpi)
        # 第二步：在父类中激活Figure窗口
        super(MyFigure, self).__init__(self.fig)  # 此句必不可少，否则不能显示图形
        self.ax = self.fig.add_subplot(111)#放置坐标系
        self.ax.plot([0,1,2,3],[0,1,2,3],'c--')

将子类创建完毕后，我们需要进行实例化，然后将其放置到容器中，这里代码还是直接搬了一部分：

from PyQt5 import QtCore, QtGui, QtWidgets

import matplotlib
matplotlib.use("Qt5Agg")  # 声明使用QT5
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
import numpy as np
 
class Ui_MainWindow(object):
    def setupUi(self, MainWindow):
        MainWindow.setObjectName("MainWindow")
        MainWindow.resize(800, 600)
        self.centralwidget = QtWidgets.QWidget(MainWindow)
        self.centralwidget.setObjectName("centralwidget")
        self.groupBox = QtWidgets.QGroupBox(self.centralwidget)
        self.groupBox.setGeometry(QtCore.QRect(39, 29, 731, 421))
        self.groupBox.setObjectName("groupBox")
        MainWindow.setCentralWidget(self.centralwidget)
        self.menubar = QtWidgets.QMenuBar(MainWindow)
        self.menubar.setGeometry(QtCore.QRect(0, 0, 800, 23))
        self.menubar.setObjectName("menubar")
        MainWindow.setMenuBar(self.menubar)
        self.statusbar = QtWidgets.QStatusBar(MainWindow)
        self.statusbar.setObjectName("statusbar")
        MainWindow.setStatusBar(self.statusbar)
 
        self.retranslateUi(MainWindow)
        QtCore.QMetaObject.connectSlotsByName(MainWindow)
 
 		self.fig = MyFigure(3, 3, 100)
        self.fig.suptitle("testfig")
        QtWidgets.QGridLayout(self.groupBox).addWidget(self.fig)
        
    def retranslateUi(self, MainWindow):
        _translate = QtCore.QCoreApplication.translate
        MainWindow.setWindowTitle(_translate("MainWindow", "MainWindow"))
        self.groupBox.setTitle(_translate("MainWindow", "GroupBox"))
 
 
class MyFigure(FigureCanvas):
    def __init__(self, width=5, height=4, dpi=100):
        # 第一步：创建一个创建Figure
        self.fig = Figure(figsize=(width, height), dpi=dpi)
        # 第二步：在父类中激活Figure窗口
        super(MyFigure, self).__init__(self.fig)  # 此句必不可少，否则不能显示图形
        self.ax = self.fig.add_subplot(111)#放置坐标系
        self.ax.plot([0,1,2,3],[0,1,2,3],'c--')
 
if __name__ == "__main__":
    import sys
    app = QtWidgets.QApplication(sys.argv)
    QMainWindow = QtWidgets.QMainWindow()
    ui = Ui_MainWindow()
    ui.setupUi(QMainWindow)
    QMainWindow.show()
    sys.exit(app.exec_())

使用matplotlib绘制动态曲线

使用matplotlib绘制动态曲线目前个人了解到的有4中方式，其中有两种我并未实践过，就不多进行介绍，大家可以参考这篇博文

1. 通过清理绘图区重绘

这种方式和第二种方式我都没进行过实际测试，只简单陈述一下：
大体就是先清除绘图区，再plot，再重绘并刷新

		self.axes.cla()  # 清除绘图区
        self.axes.plot(t, s, 'o-r', linewidth=0.5)
        self.figs.canvas.draw()  # 这里注意是画布重绘，self.figs.canvas
        self.figs.canvas.flush_events()  # 画布刷新self.figs.canvas

2.通过清理画布重绘

单纯清除绘图区无法改变背景等设置，如果需要改变背景，那么需要清理画布，只需要改为：

self.figs.clf()  # 清理画布，这里是clf()
        self.axes = self.figs.add_subplot(111)  # 清理画布后必须重新添加绘图区
        self.axes.plot(t, s, 'o-r', linewidth=0.5)
        self.figs.canvas.draw()  # 这里注意是画布重绘，self.figs.canvas
        self.figs.canvas.flush_events()  # 画布刷新self.figs.canvas

3.通过设置数据重绘

这个是我一开始使用PyQt绘制动态曲线使用的方法，但是这个方法我只能实现一个动态曲线的绘制，而使用姿态球我需要进行fill填充操作，因此最后改为animation实现。
这个设置数据重绘我只实现了基于matplotlib.lines.Line2D的重绘，大致就是：
先导入这个类

from matplotlib.lines import Line2D

然后在前面实现的MyFigure类中加入该曲线，这里方便看又把代码搬过来了。

class MyFigure(FigureCanvas):
    def __init__(self, width=5, height=4, dpi=100):
        # 第一步：创建一个创建Figure
        self.fig = Figure(figsize=(width, height), dpi=dpi)
        # 第二步：在父类中激活Figure窗口
        super(MyFigure, self).__init__(self.fig)  # 此句必不可少，否则不能显示图形
        self.ax = self.fig.add_subplot(111)#放置坐标系

F = MyFigure()
xlist = [i for i in range(0,4,1)]
ylist = [2*i  for i in range(0,4,1)]
lineaz = Line2D(xlist, ylist)
lineaz.set_color('b')
F.ax.add_line(lineaz)

完成初始绘制之后，要想更新曲线，只需要重新设置x和y的值即可，不过如果想要写随时间变化的曲线，还需要做的一件事就是更改显示的范围，这个就是最后一行代码实现的功能。

lineaz.set_xdata(new_xlist)
lineaz.set_ydata(new_ylist)
F.ax.set_xlim(new_xlist[0],new_xlist[-1])

4.通过animation重绘

这个地方我在fill那里有个操作还没弄明白，先挖个坑，有时间再填

姿态球实现

姿态球表示

这个是借鉴dji的姿态球写的，大致是使用单独线段/箭头在极坐标中画出Yaw角，然后使用分界线的倾斜程度表示Roll角，根据填充比例表示Pitch角。示意图大概就是：

算法逻辑

首先偏航角Yaw和剩余两个角度是分开计算的，偏航角Yaw非常容易表示，只需要从原点到(Yaw,设置的最大半径)连线即可。
然后的Roll角和Pitch角。Roll角和Pitch角是关联表示的，总体就是涂蓝的区域，而Roll角可以通过绿色线段的倾斜程度表示。
这个区域的计算逻辑是：

1. 根据Pitch角，将其与90度对比，取得比值再乘以极坐标图的最大半径获得值x，那么(0,x)就为绿色直线上的点。
2. 通过三角函数关系解算出直线与最大半径圆的焦点坐标，示意图大概是这样
3. 根据解算出的交点角度，在中间划分一定数目的点，然后连接线段/进行填充

代码

这个先不写，实际实现有点小问题，好像必须是严格的Pitch和Roll角才可以正常运行。

实现结果