mpython.py: Accelerometer类添加欧拉角计算

labplus-cn · Jan 9, 2021 · 6659b30 · 6659b30
1 parent bbd54a5
commit 6659b30
Showing 1 changed file with 14 additions and 0 deletions.
diff --git a/port/boards/mpython/modules/mpython.py b/port/boards/mpython/modules/mpython.py
@@ -335,6 +335,20 @@ def get_z(self):
         else:
             raise Exception("i2c read/write error!")
 
+    def roll_pitch_angle(self):
+        x, y, z = self.get_x(), self.get_y(), -self.get_z()
+        # vector normalize
+        mag = math.sqrt(x ** 2 + y ** 2+z ** 2)
+        x /= mag
+        y /= mag
+        z /= mag
+        roll = math.degrees(-math.asin(y))
+        pitch = math.degrees(math.atan2(x, z))
+
+        return roll, pitch
+
+
+
 # 3 axis accelerometer
 accelerometer = Accelerometer()