quadplotter.py 文件源码

def update(self, quad):
        """
        Set the new position and rotation of the quadcopter: ``pos`` is a tuple
        (x, y, z), and rpy is a tuple (roll, pitch, yaw) expressed in
        *radians*
        """
        x, y, z = quad.position
        roll, pitch, yaw = np.rad2deg(quad.rpy)
        self.quadplot.resetTransform()
        self.quadplot.translate(x, y, z)
        self.quadplot.rotate(roll, 1, 0, 0, local=True)
        self.quadplot.rotate(pitch, 0, 1, 0, local=True)
        self.quadplot.rotate(yaw, 0, 0, 1, local=True)
        #
        self.label_t.setText('t = %5.2f' % quad.t)