Skip to content

Commit

Permalink
Adjust scale for acc and gyro
Browse files Browse the repository at this point in the history
  • Loading branch information
iory committed Jan 16, 2024
1 parent fba1cb8 commit 87da974
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 2 deletions.
9 changes: 9 additions & 0 deletions rcb4/armh7interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -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 = [
Expand Down Expand Up @@ -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)
Expand Down
33 changes: 33 additions & 0 deletions rcb4/units.py
Original file line number Diff line number Diff line change
@@ -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
5 changes: 3 additions & 2 deletions ros/kxr_controller/node_scripts/rcb4_ros_bridge.py
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down

0 comments on commit 87da974

Please sign in to comment.