lsm303d.py 文件源码

def getTiltHeading(self):
        magValue=self.getMag()
        accelValue=self.getRealAccel()

        X=self.X
        Y=self.Y
        Z=self.Z

        pitch = math.asin(-accelValue[X])

        print(accelValue[Y],pitch,math.cos(pitch),accelValue[Y]/math.cos(pitch),math.asin(accelValue[Y]/math.cos(pitch)))
        roll = math.asin(accelValue[Y]/math.cos(pitch))

        xh = magValue[X] * math.cos(pitch) + magValue[Z] * math.sin(pitch)
        yh = magValue[X] * math.sin(roll) * math.sin(pitch) + magValue[Y] * math.cos(roll) - magValue[Z] * math.sin(roll) * math.cos(pitch)
        zh = -magValue[X] * (roll) * math.sin(pitch) + magValue[Y] * math.sin(roll) + magValue[Z] * math.cos(roll) * math.cos(pitch)
        heading = 180 * math.atan2(yh, xh)/math.pi

        if (yh >= 0):
            return heading
        else:
            return (360 + heading)