def read(self):
try: # keep running
while True:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp), end='')
return data
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
print("\nBye BeagleBone!")
# header,,
print(" Accel XYZ (m/s^2) |" " Gyro XYZ (deg/s) |", end='')
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
i = 0
avg = 0
save = []
sigma = 0
cov = 0
try: # keep running
while True:
while i < 100:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read_accel_data()
save.append(data[1])
avg = avg + data[1]
i = i + 1
avg = avg / 100
print('doing')
time.sleep(0.01)
print('done')
for i in range(0, len(save)):
sigma = sigma + ((save[i] - avg)**2)
stdv = math.sqrt(sigma / 100)
cov = stdv**2
cov = [[cov, 0, 0], [0, cov, 0], [0, 0, cov]]
print('done1')
if rcpy.get_state() == rcpy.RUNNING:
def getTemp():
temp = mpu9250.read_imu_temp() # returns just temperature (deg C)
return(temp)
rcpy.set_state(rcpy.RUNNING)
mpu9250.initialize(enable_magnetometer = True)
print("Press Ctrl-C to exit")
# header
print(" Accel XYZ (m/s^2) |"
" Gyro XYZ (deg/s) |", end='')
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
try: # keep running
while True:
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp),
end='')
time.sleep(.5) # sleep some
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
def main():
# Parse command line
try:
opts, args = getopt.getopt(sys.argv[1:], "hm", ["help"])
except getopt.GetoptError as err:
# print help information and exit:
print('rcpy_test_imu: illegal option {}'.format(sys.argv[1:]))
usage()
sys.exit(2)
# defaults
enable_magnetometer = False
for o, a in opts:
if o in ("-h", "--help"):
usage()
sys.exit()
elif o == "-m":
enable_magnetometer = True
else:
assert False, "Unhandled option"
# set state to rcpy.RUNNING
rcpy.set_state(rcpy.RUNNING)
# no magnetometer
mpu9250.initialize(enable_magnetometer = enable_magnetometer)
# message
print("try 'python rcpy_test_imu -h' to see other options")
print("Press Ctrl-C to exit")
# header
print(" Accel XYZ (m/s^2) |"
" Gyro XYZ (deg/s) |", end='')
if enable_magnetometer:
print(" Mag Field XYZ (uT) |", end='')
print(' Temp (C)')
try:
# keep running
while True:
# running
if rcpy.get_state() == rcpy.RUNNING:
temp = mpu9250.read_imu_temp()
data = mpu9250.read()
if enable_magnetometer:
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |'
' {3:6.1f}').format(data['accel'],
data['gyro'],
data['mag'],
temp),
end='')
else:
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
' {2:6.1f}').format(data['accel'],
data['gyro'],
temp),
end='')
# sleep some
time.sleep(.5)
except KeyboardInterrupt:
# Catch Ctrl-C
pass
finally:
# say bye
print("\nBye Beaglebone!")
def test1():
N = 1
try:
# no magnetometer
mpu9250.initialize(enable_magnetometer = False)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': False, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Temp (C)')
for i in range(N):
(ax,ay,az) = mpu9250.read_accel_data()
(gx,gy,gz) = mpu9250.read_gyro_data()
temp = mpu9250.read_imu_temp()
print(('\r{:6.2f} {:6.2f} {:6.2f} |' +
'{:6.1f} {:6.1f} {:6.1f} | {:6.1f}')
.format(ax, ay, az,
gx, gy, gz, temp),
end='')
time.sleep(1)
with pytest.raises(mpu9250.error):
mpu9250.read_mag_data()
# consolidated read function
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |')
.format(data['accel'],
data['gyro']),
end='')
time.sleep(1)
# with magnetometer
mpu9250.initialize(enable_magnetometer = True)
conf = mpu9250.get()
assert conf == {'orientation': 136, 'accel_dlpf': 2, 'gyro_dlpf': 2, 'compass_time_constant': 5.0, 'enable_fusion': False, 'enable_dmp': False, 'enable_magnetometer': True, 'accel_fsr': 1, 'dmp_sample_rate': 100, 'show_warnings': False, 'gyro_fsr': 2, 'dmp_interrupt_priority': 98}
print('\n Accel XYZ(m/s^2) | Gyro XYZ (rad/s) | Mag Field XYZ(uT) | Temp (C)')
for i in range(N):
(ax,ay,az) = mpu9250.read_accel_data()
(gx,gy,gz) = mpu9250.read_gyro_data()
(mx,my,mz) = mpu9250.read_mag_data()
temp = mpu9250.read_imu_temp()
print(('\r{:6.2f} {:6.2f} {:6.2f} |' +
'{:6.1f} {:6.1f} {:6.1f} |'
'{:6.1f} {:6.1f} {:6.1f} | {:6.1f}')
.format(ax, ay, az,
gx, gy, gz,
mx, my, mz, temp),
end='')
time.sleep(1)
# consolidated read function
for i in range(N):
data = mpu9250.read()
print(('\r{0[0]:6.2f} {0[1]:6.2f} {0[2]:6.2f} |'
'{1[0]:6.1f} {1[1]:6.1f} {1[2]:6.1f} |'
'{2[0]:6.1f} {2[1]:6.1f} {2[2]:6.1f} |')
.format(data['accel'],
data['gyro'],
data['mag']),
end='')
time.sleep(1)
except (KeyboardInterrupt, SystemExit):
pass
finally:
mpu9250.power_off()