# 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