# V0.7 25th June 2015 Adapted for new MPU9x50 interface
from machine import Pin
import utime as time
from mpu9150 import MPU9150
from fusion import Fusion
imu = MPU9150('X')
fuse = Fusion()
# Choose test to run
Timing = True
if Timing:
accel = imu.accel.xyz
gyro = imu.gyro.xyz
start = time.ticks_us() # Measure computation time only
fuse.update_nomag(accel, gyro) # 979μs on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update_nomag(imu.accel.xyz, imu.gyro.xyz)
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
time.sleep_ms(20)
count += 1
imu = MPU9250(i2c)
print(imu.acceleration)
print(imu.gyro)
print(imu.magnetic)
from fusion import Fusion
fuse = Fusion()
# Choose test to run
Timing = True
if Timing:
accel = imu.acceleration
gyro = imu.gyro
start = time.ticks_us() # Measure computation time only
fuse.update_nomag(accel, gyro) # 979μs on Pyboard
t = time.ticks_diff(time.ticks_us(), start)
print("Update time (uS):", t)
count = 0
while True:
fuse.update_nomag(imu.acceleration, imu.gyro)
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
time.sleep_ms(20)
count += 1
'''
This driver is notsupported by the fusion library
import time
from imu import MPU6050
import deltat
from fusion import Fusion
i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(19))
i2c.scan()
imu = MPU6050(i2c)
fuse = Fusion()
print(imu.accel.xyz)
while (True):
time.sleep_ms(500)
print(imu.accel.xyz)
while True:
fuse.update_nomag(imu.accel.xyz, imu.gyro.xyz) # Note blocking mag read
if count % 50 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
time.sleep_ms(20)
count += 1
from machine import TouchPad, Pin
t = TouchPad(Pin(4))
t.config(500)
t.read()
def touch():
if t.read() < 300:
return True
## Feedback Setup
fbk = hebi.GroupFeedback(GroupSB.size)
GroupSB.feedback_frequency = 200.0
#x = -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll)
#y = -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll)
#z = cos(pitch)sin(roll)
tStart = 0
tStop = 0
count = 0
while (1):
fbk = GroupSB.get_next_feedback(reuse_fbk=fbk)
tStop = timeit.default_timer()
#ts = tStop - tStart
fuse.update_nomag(fbk.accelerometer[0], fbk.gyro[0], tStop)
tStart = timeit.default_timer()
x = -m.cos(fuse.heading) * m.sin(fuse.pitch) - m.sin(fuse.heading) * m.cos(
fuse.roll)
y = -m.sin(fuse.heading) * m.sin(fuse.pitch) + m.cos(fuse.heading) * m.cos(
fuse.roll)
z = m.cos(fuse.pitch) * m.sin(fuse.roll)
if count % 50 == 0:
print("ts: {:7.3f}".format(tStop))
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
print("X, Y, Z: {:7.3f} {:7.3f} {:7.3f}".format(x, y, z))
print("Accel: {:7.3f} {:7.3f} {:7.3f}".format(fbk.accelerometer[0][0],
fbk.accelerometer[0][1],
lsmsensor.accelerometer_data_rate = Rate.RATE_1_66K_HZ
# sensor.accelerometer_data_rate = Rate.RATE_12_5_HZ
print("Accelerometer rate set to: %d HZ" %
Rate.string[lsmsensor.accelerometer_data_rate])
lsmsensor.gyro_data_rate = Rate.RATE_1_66K_HZ
print("Gyro rate set to: %d HZ" % Rate.string[lsmsensor.gyro_data_rate])
fuse = Fusion()
uart = UART(4)
uart.init(115200, bits=8, parity=None, stop=1)
count = 0
while True:
imu_accel_xyz = lsmsensor.acceleration
tmpgyro = lsmsensor.gyro
tmpgyro = (math.degrees(tmpgyro[0]), math.degrees(tmpgyro[1]),
math.degrees(tmpgyro[2]))
imu_gyro_xyz = tmpgyro
imu_mag_xyz = magsensor.magnetic
#fuse.update(imu_accel_xyz, imu_gyro_xyz, imu_mag_xyz) # Note blocking mag read
fuse.update_nomag(imu_accel_xyz, imu_gyro_xyz)
if count % 5 == 0:
print("Heading, Pitch, Roll: {:7.3f} {:7.3f} {:7.3f}".format(
fuse.heading, fuse.pitch, fuse.roll))
uart.write("{:7.3f},{:7.3f},{:7.3f}\n".format(fuse.heading, fuse.pitch,
fuse.roll))
time.sleep_ms(10)
count += 1
