|
#!/usr/bin/env python3 |
|
|
|
import time |
|
import struct |
|
import rospy |
|
import numpy as np |
|
from sensor_msgs.msg import Imu |
|
from tf.transformations import quaternion_about_axis |
|
|
|
import smbus2 as smbus |
|
import math |
|
|
|
imu_pub = None |
|
IMU_FRAME = None |
|
|
|
# Register |
|
MPU9250_RA_PWR_MGMT_1 = 0x6B |
|
MPU9250_RA_PWR_MGMT_2 = 0x6C |
|
|
|
|
|
def read_byte(reg): |
|
return bus.read_byte_data(address, reg) |
|
|
|
def read_word(reg): |
|
h = bus.read_byte_data(address, reg) |
|
l = bus.read_byte_data(address, reg+1) |
|
value = (h << 8) + l |
|
return value |
|
|
|
def read_word_2c(reg): |
|
val = read_word(reg) |
|
if (val >= 0x8000): |
|
return -((65535 - val) + 1) |
|
else: |
|
return val |
|
|
|
def dist(a,b): |
|
return math.sqrt((a*a)+(b*b)) |
|
|
|
def get_y_rotation(x,y,z): |
|
radians = math.atan2(x, dist(y,z)) |
|
return -math.degrees(radians) |
|
|
|
def get_x_rotation(x,y,z): |
|
radians = math.atan2(y, dist(x,z)) |
|
return math.degrees(radians) |
|
|
|
MPU9250_ADDRESS_AD0_LOW = 0x68 # address pin low (GND), default for InvenSense evaluation board |
|
MPU9250_ADDRESS_AD0_HIGH = 0x69 # address pin high (VCC) |
|
MPU9250_DEFAULT_ADDRESS = MPU9250_ADDRESS_AD0_LOW |
|
|
|
bus = smbus.SMBus(1) # bus = smbus.SMBus(0) fuer Revision 1 |
|
address = MPU9250_DEFAULT_ADDRESS # via i2cdetect |
|
|
|
bus.write_byte_data(address, MPU9250_RA_PWR_MGMT_1, 0) |
|
|
|
# Consts from: https://github.com/linorobot/linorobot/tree/master/teensy/firmware/lib/imu |
|
# Code from: https://github.com/OSUrobotics/mpu_6050_driver/blob/master/scripts/imu_node.py |
|
|
|
# http://www.sureshjoshi.com/embedded/invensense-imus-what-to-know/ |
|
# https://stackoverflow.com/questions/19161872/meaning-of-lsb-unit-and-unit-lsb |
|
# MPU9250 https://www.invensense.com/wp-content/uploads/2015/02/MPU-9150-Datasheet.pdf |
|
|
|
G_TO_ACCEL = 9.81 |
|
MGAUSS_TO_UTESLA = 0.1 |
|
UTESLA_TO_TESLA = 0.000001 |
|
|
|
ACCEL_SCALE = 1.0 / 16384 # * G_TO_ACCEL # m/s^2 |
|
GYRO_SCALE = math.radians(1.0 / 131) # rad/s |
|
GYRO_SCALE = 1.0 / 131 # rad/s |
|
|
|
MAG_SCALE = 0.6 # uT/LSB |
|
|
|
# accelerometer |
|
MPU9250_RA_ACCEL_XOUT = 0x3B |
|
MPU9250_RA_ACCEL_YOUT = 0x3D |
|
MPU9250_RA_ACCEL_ZOUT = 0x3F |
|
|
|
# gyro |
|
MPU9250_RA_GYRO_XOUT = 0x43 |
|
MPU9250_RA_GYRO_YOUT = 0x45 |
|
MPU9250_RA_GYRO_ZOUT = 0x47 |
|
|
|
# magnetometer |
|
MPU9150_RA_MAG_ADDRESS = 0x0C |
|
MPU9150_RA_MAG_XOUT = 0x04 |
|
MPU9150_RA_MAG_YOUT = 0x06 |
|
MPU9150_RA_MAG_ZOUT = 0x08 |
|
|
|
# temperature |
|
MPU9250_RA_TEMP_OUT = 0x41 |
|
|
|
def publish_imu(timer_event): |
|
imu_msg = Imu() |
|
imu_msg.header.frame_id = "map" |
|
|
|
# Read the acceleration vals |
|
# LSB/g (least significant bit per g) |
|
accel_x = read_word_2c(MPU9250_RA_ACCEL_XOUT) * ACCEL_SCALE |
|
accel_y = read_word_2c(MPU9250_RA_ACCEL_YOUT) * ACCEL_SCALE |
|
accel_z = read_word_2c(MPU9250_RA_ACCEL_ZOUT) * ACCEL_SCALE |
|
|
|
|
|
# Calculate a quaternion representing the orientation |
|
accel = accel_x, accel_y, accel_z |
|
ref = np.array([0, 0, 1]) |
|
acceln = accel / (np.linalg.norm(accel) + 1e-10) |
|
axis = np.cross(acceln, ref) |
|
angle = np.arccos(np.dot(acceln, ref)) |
|
orientation = quaternion_about_axis(angle, axis) |
|
|
|
# Read the gyro vals |
|
# 16-bit signed integer container for X, Y, Z -axis rotation |
|
gyro_x = read_word_2c(MPU9250_RA_GYRO_XOUT) * GYRO_SCALE # rad/s |
|
gyro_y = read_word_2c(MPU9250_RA_GYRO_YOUT) * GYRO_SCALE |
|
gyro_z = read_word_2c(MPU9250_RA_GYRO_ZOUT) * GYRO_SCALE |
|
|
|
print([gyro_x, gyro_y, gyro_z]) |
|
|
|
|
|
# Get current internal temperature |
|
# Temperature reading in 16-bit 2's complement format |
|
# celsius degrees |
|
temperature = read_word_2c(MPU9250_RA_TEMP_OUT) |
|
|
|
|
|
# TODO: Magnetometer |
|
# https://github.com/linorobot/linorobot/blob/master/teensy/firmware/lib/imu/MPU9250.cpp#L1840 |
|
|
|
# Load up the IMU message |
|
o = imu_msg.orientation |
|
o.x, o.y, o.z, o.w = orientation |
|
|
|
imu_msg.linear_acceleration.x = accel_x |
|
imu_msg.linear_acceleration.y = accel_y |
|
imu_msg.linear_acceleration.z = accel_z |
|
|
|
imu_msg.angular_velocity.x = gyro_x |
|
imu_msg.angular_velocity.y = gyro_y |
|
imu_msg.angular_velocity.z = gyro_z |
|
|
|
imu_msg.header.stamp = rospy.Time.now() |
|
|
|
imu_pub.publish(imu_msg) |
|
|
|
|
|
if __name__ == '__main__': |
|
rospy.init_node('imu_node') |
|
|
|
frequency = rospy.get_param('~frequency', 50) # Hz |
|
IMU_FRAME = rospy.get_param('~imu_frame', 'imu_link') # |
|
|
|
|
|
imu_pub = rospy.Publisher('imu/data_raw', Imu, queue_size=16) |
|
imu_timer = rospy.Timer(rospy.Duration(1.0 / frequency), publish_imu) |
|
rospy.spin() |
