From d774add904dba57278b1553abff0e8bf6421c51d Mon Sep 17 00:00:00 2001
From: Naoya Yamaguchi <708yamaguchi@gmail.com>
Date: Wed, 13 Mar 2024 23:29:56 +0900
Subject: [PATCH] Fix MPU9250 acceleration measurement range

---
 rcb4/armh7interface.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index 508a0926..38f788fd 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -695,7 +695,10 @@ def read_rpy(self):
 
     def read_imu_data(self):
         cs = self.memory_cstruct(Madgwick, 0)
+        # MPU9250 acceleration measurement range is +-8g
         acc = convert_data(cs.acc, 8)
+        g = 9.81
+        acc = acc * g
         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