def callimu():
imu = LSM6DS33()
imu.enable()
while True:
ryp = getrpy(imu.get_accelerometer_g_forces())
q = quaternion_from_euler(ryp[0], ryp[1], ryp[2])
print("Accelerometer (G):", ryp, q, imu.get_gyro_angular_velocity())
sleep(1)
def __init__(self):
super(IMU, self).__init__()
self.lsm6ds33 = LSM6DS33()
self.gyroAccelEnabled = False
self.lps25h = LPS25H()
self.barometerEnabled = False
self.lis3mdl = LIS3MDL()
self.magnetometerEnabled = False
def __init__(self, busId=1, sa0=SA0_HIGH):
""" Compose other classes
busId refers to the I2C bus to use (default 1)
sa0 refers to the I2C Master/Slave address pin status
SA0_HIGH (default) == pullup to vcc => Master addresses
SA0_LOW == grounded => Slave addresses
"""
self.imu = LSM6DS33(busId=busId, sa0=sa0)
self.magnet = LIS3MDL(busId, sa0=sa0)
self.baro = LPS25H(busId, sa0=sa0)
pitch = -float(words[1]) * degrees2rad
roll = float(words[2]) * degrees2rad
ret = [roll, pitch, yaw]
return ret
roll = 0
pitch = 0
yaw = 0
seq = 0
accel_factor = 9.806 / 256.0 # sensor reports accel as 256.0 = 1G (9.8m/s^2). Convert to m/s^2.
rospy.loginfo("Giving the razor IMU board 5 seconds to boot...")
rospy.sleep(5) # Sleep for 5 seconds to wait for the board to boot
imu = LSM6DS33()
imu.enable()
while not rospy.is_shutdown():
linvel = imu.getAccelerometer3Angles()
ryp = getrpy(linvel)
avel = imu.get_gyro_angular_velocity()
q = quaternion_from_euler(ryp[0], ryp[1], ryp[2])
# Publish message
# AHRS firmware accelerations are negated
# This means y and z are correct for ROS, but x needs reversing
imuMsg.linear_acceleration.x = -linvel[0]
imuMsg.linear_acceleration.y = linvel[1]
imuMsg.linear_acceleration.z = linvel[2]
imuMsg.angular_velocity.x = avel[0]
from lsm6ds33 import LSM6DS33 lsm = LSM6DS33() lsm.enableLSM() while True: print(lsm.getAllRaw())
#print('off')
# Initialise GPIO components
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
pin1 = 17 # pin 11 on the RP board
#pin2 = 27
GPIO.setup(pin1, GPIO.OUT)
#GPIO.setup(pin2, GPIO.OUT)
# Initialise ADDA component (force sensor)
board = Board()
# Initialise (accelerometer)
lsm6ds33 = LSM6DS33()
lsm6ds33.enable()
# Initialise bluetooth socket
server_sock = BluetoothSocket(RFCOMM)
server_sock.bind(("", PORT_ANY))
server_sock.listen(1)
port = server_sock.getsockname()[1]
uuid = "94f39d29-7d6d-437d-973b-fba39e49d4ee"
advertise_service(
server_sock,
"Rasp Pi3 Master",
service_id=uuid,