RollPitchYaw.py 文件源码

def calcYaw(self, magnet, roll, pitch):
        magX = magnet[0]
        magY = magnet[1]
        magZ = magnet[2]
        rowX = magX
        rowY = -magY
        rowZ = -magZ
        row = np.matrix([[rowX], [rowY], [rowZ]])
        A = np.matrix([ \
            [math.cos(roll), math.sin(roll) * math.sin(pitch), math.cos(pitch) * math.sin(roll)] \
            , [0, math.cos(pitch), -math.sin(pitch)] \
            , [-math.sin(roll), math.sin(pitch) * math.cos(roll), math.cos(roll) * math.cos(pitch)] \
            ])
        calib = A * row
        calibX = row[0]
        calibY = row[1]
        calibZ = row[2]
        try:
            yaw = math.atan(calibY / calibX)
        except:
            if calibY > 0:
                yaw = 1.0
            else:
                yaw = -1.0
        return yaw