From d6ec7324a79cde25fc8f2063e283846fc7bd110d Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Tue, 16 Jan 2024 14:19:30 +0900
Subject: [PATCH] Adjust scale for acc and gyro

---
 rcb4/armh7interface.py                        |  9 +++++
 rcb4/units.py                                 | 33 +++++++++++++++++++
 .../node_scripts/rcb4_ros_bridge.py           |  5 +--
 3 files changed, 45 insertions(+), 2 deletions(-)
 create mode 100644 rcb4/units.py

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index c665feee..8287ed4f 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -35,6 +35,7 @@
 from rcb4.struct_header import ServoStruct
 from rcb4.struct_header import SystemStruct
 from rcb4.struct_header import WormmoduleStruct
+from rcb4.units import convert_data
 
 
 armh7_variable_list = [
@@ -638,6 +639,14 @@ def read_rpy(self):
         cs = self.memory_cstruct(Madgwick, 0)
         return [cs.roll, cs.pitch, cs.yaw]
 
+    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)
+        gyro = convert_data(cs.gyro, 2000)
+        gyro = np.deg2rad(gyro)
+        return q_wxyz, acc, gyro
+
     def gyro_norm_vector(self):
         g = self.memory_cstruct(Madgwick, 0).gyro
         n = np.sqrt(g[0] ** 2 + g[1] ** 2 + g[2] ** 2)
diff --git a/rcb4/units.py b/rcb4/units.py
new file mode 100644
index 00000000..c037dc10
--- /dev/null
+++ b/rcb4/units.py
@@ -0,0 +1,33 @@
+import numpy as np
+
+
+def convert_data(raw_data, range_g):
+    """Convert raw 16-bit signed integer data
+
+    Convert raw 16-bit signed integer accelerometer data
+    to actual values.
+
+    Parameters
+    ----------
+    raw_data : int or np.ndarray
+        Raw data, expected as 16-bit signed integers.
+    range_g : float
+        The measuring range of the accelerometer in ±g, e.g., 8, 16, 32, 64.
+
+    Returns
+    -------
+    float or np.ndarray
+        The actual values in g. Returns a float if the input is
+        a scalar, or np.ndarray if the input is an array.
+
+    Examples
+    --------
+    >>> convert_data(16000, 8)
+    3.91
+
+    >>> convert_data(np.array([16000, -16000]), 8)
+    np.array([3.91, -3.91])
+    """
+    max_value = 32767
+    conversion_factor = (range_g * 2) / (max_value * 2)
+    return np.array(raw_data) * conversion_factor
diff --git a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
index 3b2ef5a9..0c133351 100644
--- a/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
+++ b/ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
@@ -330,12 +330,13 @@ def create_imu_message(self):
         msg = sensor_msgs.msg.Imu()
         msg.header.frame_id = self.imu_frame_id
         msg.header.stamp = rospy.Time.now()
-        q_wxyz = self.arm.read_quaternion()
+        q_wxyz, acc, gyro = self.arm.read_imu_data()
         (msg.orientation.w, msg.orientation.x,
          msg.orientation.y, msg.orientation.z) = q_wxyz
-        _, gyro = self.arm.gyro_norm_vector()
         (msg.angular_velocity.x, msg.angular_velocity.y,
          msg.angular_velocity.z) = gyro
+        (msg.linear_acceleration.x, msg.linear_acceleration.y,
+         msg.linear_acceleration.z) = acc
         return msg
 
     def run(self):