Normalize quaternion

iory · Mar 13, 2024 · 4acb21d · 4acb21d
1 parent c2ffee4
commit 4acb21d
Showing 1 changed file with 2 additions and 0 deletions.
diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
@@ -697,6 +697,8 @@ def read_imu_data(self):
         cs = self.memory_cstruct(Madgwick, 0)
         acc = convert_data(cs.acc, 8)
         q_wxyz = np.array([cs.q0, cs.q1, cs.q2, cs.q3], dtype=np.float32)
+        norm_q = np.sqrt(np.dot(q_wxyz.T, q_wxyz))
+        q_wxyz = q_wxyz / norm_q
         gyro = convert_data(cs.gyro, 2000)
         gyro = np.deg2rad(gyro)
         return q_wxyz, acc, gyro