def __init__(self, *statevars):
name1 = 's'
port1 = '/dev/ttySAC0'
self.statevars = [] # example: ['roll', 'pitch', 'yaw']
for state in statevars:
self.statevars.append(state)
# All states avaible:
#['roll', 'pitch', 'yaw']
#['health', 'roll', 'pitch', 'yaw'] #'mag_proc_x', 'mag_proc_y', 'mag_proc_z', 'mag_raw_x', 'mag_raw_y', 'mag_raw_z', #'accel_raw_x', 'accel_raw_y', 'accel_raw_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z', 'gyro_proc_x', 'gyro_proc_y', #'gyro_proc_z', 'accel_raw_time', 'accel_proc_time', 'euler_time']
self.s1 = um7.UM7(name1, port1, self.statevars, baud=115200)
try:
print('IMU initialition process:')
print('GET_FW_REVISION=' + '{}'.format(self.s1.get_fw_revision()))
print('ZERO_GYROS ' + 'ok.' if self.s1.zero_gyros() else 'failed.')
print('RESET_EKF ' + 'ok.' if self.s1.reset_ekf() else 'failed.')
#s1.reset_to_factory()
s1.set_mag_reference()
s1.set_home_position()
except:
print('------------!ERROR occured!--------------\n')
# readings view format:
self.fs = ''
self.hs = ''
for i in self.statevars:
self.hs += '{:>9.9s} '
if i == 'health':
self.fs += ' {0['+i+']:08b} '
else:
self.fs += '{0['+i+']:9.3f} '
self.sv = ['roll', 'pitch', 'yaw']
def __init__(self, *statevars):
import um7
self.gyro_integrator = Integrator()
self.acc_integrator = Integrator()
self.vel_integrator = Integrator()
self.i = 0
self.gyro_filter = 0.2
self.integrator = Integrator()
name1 = 's'
port1 = '/dev/ttySAC0'
self.statevars = [] # example: ['roll', 'pitch', 'yaw']
for state in statevars:
self.statevars.append(state)
# All states avaible:
# ['roll', 'pitch', 'yaw']
# ['health', 'roll', 'pitch', 'yaw'] #'mag_proc_x', 'mag_proc_y', 'mag_proc_z', 'mag_raw_x', 'mag_raw_y',
# 'mag_raw_z', #'accel_raw_x', 'accel_raw_y', 'accel_raw_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z',
# 'gyro_proc_x', 'gyro_proc_y', #'gyro_proc_z', 'accel_raw_time', 'accel_proc_time', 'euler_time']
self.s1 = um7.UM7(name1, port1, self.statevars, baud=115200)
try:
print('IMU initialition process:')
# self.s1.reset_to_factory()
print('GET_FW_REVISION=' + '{}'.format(self.s1.get_fw_revision()))
print('ZERO_GYROS ' + 'ok.' if self.s1.zero_gyros() else 'failed.')
time.sleep(1.1)
print('RESET_EKF ' + 'ok.' if self.s1.reset_ekf() else 'failed.')
print('SET_MAG_REFERENCE ' + 'ok.' if self.s1.set_mag_reference() else 'failed.')
time.sleep(1.1)
# print('SET_HOME_POSITION ' + 'ok.' if self.s1.set_home_position() else 'failed.')
# print('RESET_EKF ' + 'ok.' if self.s1.reset_ekf() else 'failed.')
# print('FLASH_COMMIT ' + 'ok.' if self.s1.flash_commit() else 'failed.')
# time.sleep(3)
except:
print('------------!ERROR occured!--------------\n')
# readings view format:
self.fs = ''
self.hs = ''
for i in self.statevars:
self.hs += '{:>9.9s} '
if i == 'health':
self.fs += ' {0[' + i + ']:08b} '
else:
self.fs += '{0[' + i + ']:9.3f} '
self.sv = ['roll', 'pitch', 'yaw']
self.prev_sample = 0
self.drift = 0
self.gravity_vector = (0, 0, 0)
# init
time.sleep(1)
self.catchSamples()
angles = self.makeAnglesDict(self.getSample('roll'), self.getSample('pitch'), self.getSample('yaw'))
vec = self.makeVector(self.getSample('accel_proc_x'), self.getSample('accel_proc_y'),
self.getSample('accel_proc_z'))
self.gravity_vector = self.rotate(angles, vec, True)
def __init__(self, config):
super(UM7IMU, self).__init__()
self.gyro_integrator = Integrator()
self.acc_integrator = Integrator()
self.vel_integrator = Integrator()
self.i = 0
self.gyro_filter = 0.2
self.integrator = Integrator()
name1 = 's'
port1 = '/dev/ttySAC0'
self.statevars = ['roll', 'pitch', 'yaw', 'accel_proc_x', 'accel_proc_z', 'accel_proc_y','gyro_proc_z' ,'gyro_proc_x', 'gyro_raw_z'] # example: ['roll', 'pitch', 'yaw']
#for state in statevars:
# self.statevars.append(state)
# All states avaible:
# ['roll', 'pitch', 'yaw']
# ['health', 'roll', 'pitch', 'yaw'] #'mag_proc_x', 'mag_proc_y', 'mag_proc_z', 'mag_raw_x', 'mag_raw_y',
# 'mag_raw_z', #'accel_raw_x', 'accel_raw_y', 'accel_raw_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z',
# 'gyro_proc_x', 'gyro_proc_y', #'gyro_proc_z', 'accel_raw_time', 'accel_proc_time', 'euler_time']
self.s1 = um7.UM7(name1, port1, self.statevars, baud=115200)
try:
print('IMU initialition process:')
# self.s1.reset_to_factory()
print('GET_FW_REVISION=' + '{}'.format(self.s1.get_fw_revision()))
print('ZERO_GYROS ' + 'ok.' if self.s1.zero_gyros() else 'failed.')
time.sleep(1.1)
#print('RESET_EKF ' + 'ok.' if self.s1.reset_ekf() else 'failed.')
print('SET_MAG_REFERENCE ' + 'ok.' if self.s1.set_mag_reference() else 'failed.')
time.sleep(2.1)
# print('SET_HOME_POSITION ' + 'ok.' if self.s1.set_home_position() else 'failed.')
print('RESET_EKF ' + 'ok.' if self.s1.reset_ekf() else 'failed.')
# print('FLASH_COMMIT ' + 'ok.' if self.s1.flash_commit() else 'failed.')
# time.sleep(3)
except:
print('------------!ERROR occured!--------------\n')
# readings view format:
self.fs = ''
self.hs = ''
for i in self.statevars:
self.hs += '{:>9.9s} '
if i == 'health':
self.fs += ' {0[' + i + ']:08b} '
else:
self.fs += '{0[' + i + ']:9.3f} '
self.sv = ['roll', 'pitch', 'yaw']
def __init__(self, default_port='/dev/ttyUSB1'):
"""
@param default_port: default serial port to use for
establishing a connection to the UM7 IMU sensor.
This will be overridden by ~port param if
available.
"""
rospy.init_node('imu_um7')
self.port = rospy.get_param('~port', default_port)
self.frame_id = rospy.get_param('~frame_id', "/imu")
self.gps_frame_id = rospy.get_param('~gps_frame_id', "/gps")
self.throttle_rate = rospy.get_param('~throttle_rate', 5000)
self.reset_mag = rospy.get_param('~reset_mag', False)
self.reset_accel = rospy.get_param('~reset_accel', False)
self.mag_zero_x = rospy.get_param('~mag_zero_x', False)
self.mag_zero_y = rospy.get_param('~mag_zero_y', False)
self.mag_zero_z = rospy.get_param('~mag_zero_z', False)
rospy.loginfo("serial port: %s" % (self.port))
self.link = rospy.get_param('~link', 'imu_link')
rospy.loginfo("tf link: {}".format(self.link))
self.imu_data = Imu()
self.imu_data = Imu(header=rospy.Header(frame_id=self.link))
# These covariance calculations are based on those bound in the existing
# UM7 ROS package at:
# https://github.com/ros-drivers/um7
linear_acceleration_stdev = rospy.get_param(
"~linear_acceleration_stdev", 4.0 * 1e-3 * 9.80665)
angular_velocity_stdev = rospy.get_param("~angular_velocity_stdev",
np.deg2rad(0.06))
linear_acceleration_cov = linear_acceleration_stdev * linear_acceleration_stdev
angular_velocity_cov = angular_velocity_stdev * angular_velocity_stdev
# From the UM7 datasheet for the dynamic accuracy from the EKF.
orientation_x_stdev = rospy.get_param("~orientation_x_stdev",
np.deg2rad(3.0))
orientation_y_stdev = rospy.get_param("~orientation_y_stdev",
np.deg2rad(3.0))
orientation_z_stdev = rospy.get_param("~orientation_z_stdev",
np.deg2rad(5.0))
orientation_x_covar = orientation_x_stdev * orientation_x_stdev
orientation_y_covar = orientation_y_stdev * orientation_y_stdev
orientation_z_covar = orientation_z_stdev * orientation_z_stdev
self.imu_data.orientation_covariance = [
orientation_x_covar, 0, 0, 0, orientation_y_covar, 0, 0, 0,
orientation_z_covar
]
self.imu_data.angular_velocity_covariance = [
angular_velocity_cov, 0, 0, 0, angular_velocity_cov, 0, 0, 0,
angular_velocity_cov
]
self.imu_data.linear_acceleration_covariance = [
linear_acceleration_cov, 0, 0, 0, linear_acceleration_cov, 0, 0, 0,
linear_acceleration_cov
]
# Set up the imu data message
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=1)
# Set up the Roll-Pitch-Yaw (rpy) message
self.rpy_data = Vector3Stamped()
self.rpy_pub = rospy.Publisher('imu/rpy', Vector3Stamped, queue_size=1)
# Set up the magnometer message
self.mag_data = Vector3Stamped()
self.mag_pub = rospy.Publisher('imu/mag', Vector3Stamped, queue_size=1)
# what data to get from the UM7
self.statevars = [
'health', 'roll', 'pitch', 'yaw', 'mag_proc_x', 'mag_proc_y',
'mag_proc_z', 'accel_proc_x', 'accel_proc_y', 'accel_proc_z',
'gyro_proc_x', 'gyro_proc_y', 'gyro_proc_z', 'quat_a', 'quat_b',
'quat_c', 'quat_d', 'gps_latitude', 'gps_longitude', 'gps_altitude'
]
# Masks for parsing out the health data from the IMU
self.NUM_SATS_USED = 0b11111100000000000000000000000000
self.HDOP = 0b00000011111111110000000000000000 # Not used as of 05/23/19
self.NUM_SATS_IN_VIEW = 0b00000000000000001111110000000000
# Set up the GPS NavSatFix publisher
self.fix_data = NavSatFix()
self.fix_pub = rospy.Publisher('fix', NavSatFix, queue_size=1)
imu_connected = False
while not rospy.is_shutdown() and not imu_connected:
try:
self.driver = um7.UM7('s',
self.port,
self.statevars,
baud=115200)
imu_connected = True
rospy.loginfo("Imu initialization completed")
self.received = -1
if self.reset_mag:
self.driver.set_mag_reference()
if self.reset_accel:
cmd_seq.append(Um6Drv.CMD_SET_ACCEL_REF)
if self.mag_zero_x and self.mag_zero_y and self.mag_zero_z:
rospy.loginfo("Magnetometer calibration: %.3f %.3f %.3f",
self.mag_zero_x, self.mag_zero_y,
self.mag_zero_z)
except SerialException:
rospy.logwarn(
"Serial error communicating with IMU. Will retry in 2.0 seconds."
)
rospy.sleep(2.0)
import numpy as np
import struct
import um7
name1 = 's'
# port1 = '/dev/ttyS0'
port1 = '/dev/tty.usbserial-A903AAUJ'
statevars = []
sensor1 = um7.UM7(name1, port1, statevars, baud=115200)
np.set_printoptions(formatter={'float': lambda x: '{:11.5f}'.format(x)})
p = sensor1.readreg(um7.CREG_MISC_SETTINGS, 1)
cs = list(struct.unpack('!I', p.data))
print('CREG_MISC_SETTINGS={:032b}'.format(cs[0]))
p = sensor1.readreg(um7.CREG_ACCEL_BIAS_X, 3)
bx, by, bz = struct.unpack('!fff', p.data)
print('accel bias: {:9.2f} {:9.2f} {:9.2f}'.format(bx, by, bz))
p = sensor1.readreg(um7.CREG_ACCEL_CAL1_1, 9)
floats = struct.unpack('!fffffffff', p.data)
old_m = np.array([floats[0:3], floats[3:6], floats[6:9]])
print("accel cal matrix=")
print(old_m)
p = sensor1.readreg(um7.CREG_MAG_CAL1_1, 9)
floats = struct.unpack('!fffffffff', p.data)
old_m = np.array([floats[0:3], floats[3:6], floats[6:9]])
print("mag cal matrix=")
import um7
import time
import zmq
name = str(sys.argv[1])
port = str(sys.argv[2])
localhost = str(sys.argv[3])
measurements = list(sys.argv[4:])
timevar = name + ' time'
context = zmq.Context()
socket = context.socket(zmq.PUB)
socket.setsockopt(zmq.SNDHWM, 1)
socket.bind("tcp://127.0.0.1:" + localhost)
sensor = um7.UM7(name, port, measurements, baud=921600)
time.sleep(2)
sensor.settimer()
print 'Starting BT...'
sensor.btstart()
while not sensor.zerogyros():
pass
while not sensor.resetekf():
pass
data = {}
try:
print 'Taking data...'
while True:
while True:
newmsg = sensor.catchsample()
if newmsg: data.update(newmsg)
