211 lines
6.6 KiB
Python
211 lines
6.6 KiB
Python
"""
|
|
Example for BNO080 integrated IMU.
|
|
Available from Sparkfun.
|
|
"""
|
|
|
|
import sys
|
|
import serial
|
|
import time
|
|
import struct
|
|
import random
|
|
import math
|
|
|
|
from i2cdriver import I2CDriver
|
|
def hexdump(s):
|
|
def toprint(c):
|
|
if 32 <= c < 127:
|
|
return chr(c)
|
|
else:
|
|
return "."
|
|
def hexline(s):
|
|
return (" ".join(["%02x" % c for c in s]).ljust(49) +
|
|
"|" +
|
|
"".join([toprint(c) for c in s]).ljust(16) +
|
|
"|")
|
|
return "\n".join([hexline(s[i:i+16]) for i in range(0, len(s), 16)])
|
|
|
|
CHANNEL_COMMAND = 0
|
|
CHANNEL_EXECUTABLE = 1
|
|
CHANNEL_CONTROL = 2
|
|
CHANNEL_REPORTS = 3
|
|
CHANNEL_WAKE_REPORTS = 4
|
|
CHANNEL_GYRO = 5
|
|
|
|
# All the ways we can configure or talk to the BNO080, figure 34, page 36 reference manual
|
|
# These are used for low level communication with the sensor, on channel 2
|
|
SHTP_REPORT_COMMAND_RESPONSE = 0xF1
|
|
SHTP_REPORT_COMMAND_REQUEST = 0xF2
|
|
SHTP_REPORT_FRS_READ_RESPONSE = 0xF3
|
|
SHTP_REPORT_FRS_READ_REQUEST = 0xF4
|
|
SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8
|
|
SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9
|
|
SHTP_REPORT_BASE_TIMESTAMP = 0xFB
|
|
SHTP_REPORT_SET_FEATURE_COMMAND = 0xFD
|
|
|
|
# All the different sensors and features we can get reports from
|
|
# These are used when enabling a given sensor
|
|
SENSOR_REPORTID_ACCELEROMETER = 0x01
|
|
SENSOR_REPORTID_GYROSCOPE = 0x02
|
|
SENSOR_REPORTID_MAGNETIC_FIELD = 0x03
|
|
SENSOR_REPORTID_LINEAR_ACCELERATION = 0x04
|
|
SENSOR_REPORTID_ROTATION_VECTOR = 0x05
|
|
SENSOR_REPORTID_GRAVITY = 0x06
|
|
SENSOR_REPORTID_GAME_ROTATION_VECTOR = 0x08
|
|
SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR = 0x09
|
|
SENSOR_REPORTID_TAP_DETECTOR = 0x10
|
|
SENSOR_REPORTID_STEP_COUNTER = 0x11
|
|
SENSOR_REPORTID_STABILITY_CLASSIFIER = 0x13
|
|
SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER = 0x1E
|
|
|
|
# Record IDs from figure 29, page 29 reference manual
|
|
# These are used to read the metadata for each sensor type
|
|
FRS_RECORDID_ACCELEROMETER = 0xE302
|
|
FRS_RECORDID_GYROSCOPE_CALIBRATED = 0xE306
|
|
FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED = 0xE309
|
|
FRS_RECORDID_ROTATION_VECTOR = 0xE30B
|
|
|
|
# Command IDs from section 6.4, page 42
|
|
# These are used to calibrate, initialize, set orientation, tare etc the sensor
|
|
COMMAND_ERRORS = 1
|
|
COMMAND_COUNTER = 2
|
|
COMMAND_TARE = 3
|
|
COMMAND_INITIALIZE = 4
|
|
COMMAND_DCD = 6
|
|
COMMAND_ME_CALIBRATE = 7
|
|
COMMAND_DCD_PERIOD_SAVE = 9
|
|
COMMAND_OSCILLATOR = 10
|
|
COMMAND_CLEAR_DCD = 11
|
|
|
|
CALIBRATE_ACCEL = 0
|
|
CALIBRATE_GYRO = 1
|
|
CALIBRATE_MAG = 2
|
|
CALIBRATE_PLANAR_ACCEL = 3
|
|
CALIBRATE_ACCEL_GYRO_MAG = 4
|
|
CALIBRATE_STOP = 5
|
|
|
|
def normalize(v, tolerance=0.00001):
|
|
mag2 = sum(n * n for n in v)
|
|
if abs(mag2 - 1.0) > tolerance:
|
|
mag = math.sqrt(mag2)
|
|
v = tuple(n / mag for n in v)
|
|
return v
|
|
|
|
class BNO080:
|
|
def __init__(self, i2, a = 0x4b):
|
|
self.i2 = i2
|
|
self.a = a
|
|
self.seqno = [0] * 8
|
|
|
|
if 1:
|
|
self.sendPacket(CHANNEL_EXECUTABLE, [1])
|
|
time.sleep(0.150)
|
|
while True:
|
|
if not self.receivePacket():
|
|
break
|
|
|
|
self.setFeature(SENSOR_REPORTID_ROTATION_VECTOR, 50000)
|
|
|
|
def read_quaternion(self):
|
|
while True:
|
|
r = self.receivePacket()
|
|
while r:
|
|
tag = r[0]
|
|
r = r[1:]
|
|
if tag == 0xfb:
|
|
# print("Time %d" % struct.unpack("I", r[:4]))
|
|
r = r[4:]
|
|
elif tag == SHTP_REPORT_COMMAND_RESPONSE:
|
|
r = r[16:]
|
|
elif tag == 0xfc:
|
|
# print("Get Feature Response")
|
|
r = r[17:]
|
|
elif tag == SENSOR_REPORTID_ACCELEROMETER:
|
|
r = r[10:]
|
|
elif tag == SENSOR_REPORTID_ROTATION_VECTOR:
|
|
(_,_,_,i,j,k,w,_) = struct.unpack("<BBBhhhhh", r[:14])
|
|
Q14 = 2 ** -14
|
|
# print((i * Q14, j * Q14, k * Q14, w * Q14))
|
|
return (w * Q14, i * Q14, j * Q14, k * Q14)
|
|
r = r[14:]
|
|
else:
|
|
assert 0, "Bad tag %#x" % tag
|
|
|
|
def setFeature(self, reportID, timeBetweenReports, specificConfig = 0):
|
|
p = struct.pack("<BBBHIII",
|
|
SHTP_REPORT_SET_FEATURE_COMMAND,
|
|
reportID,
|
|
0, 0,
|
|
timeBetweenReports,
|
|
0, specificConfig)
|
|
# print(hexdump(p))
|
|
|
|
self.sendPacket(CHANNEL_CONTROL, p)
|
|
|
|
def sendPacket(self, channel, data):
|
|
# print('send on', channel, 'seq', self.seqno[channel])
|
|
self.i2.start(self.a, 0)
|
|
self.i2.write(struct.pack("<HBB", 4 + len(data), channel, self.seqno[channel]))
|
|
self.i2.write(data)
|
|
self.i2.stop()
|
|
self.seqno[channel] += 1
|
|
|
|
def receivePacket(self):
|
|
if not self.i2.start(self.a, 1):
|
|
self.i2.stop()
|
|
return None
|
|
hdr = self.i2.read(4)
|
|
self.i2.stop()
|
|
length, channel, sequence = (struct.unpack("<HBB", hdr))
|
|
length &= 0x7fff
|
|
# print()
|
|
# print('length', length)
|
|
# print('channel', channel)
|
|
# print('sequence', sequence)
|
|
if length == 0 or channel not in range(8):
|
|
return None
|
|
|
|
# self.seqno[channel] = sequence + 1
|
|
|
|
self.i2.start(self.a, 1)
|
|
data = self.i2.read(length)
|
|
self.i2.stop()
|
|
# print(len(data), repr(data))
|
|
# print(hexdump(data[4:]))
|
|
return data[4:]
|
|
|
|
def showpacket(self, data):
|
|
tag = data[0]
|
|
print('tag', tag)
|
|
r = data[1:]
|
|
if tag == 0:
|
|
self.showpacket_00(r)
|
|
else:
|
|
assert False, "Cannot show packet %02x" % tag
|
|
|
|
def showpacket_00(self, ad):
|
|
while ad:
|
|
(T, L) = struct.unpack("BB", ad[:2])
|
|
V = ad[2:2+L]
|
|
decoder = {
|
|
1: lambda: ("GUID", struct.unpack("I", V)),
|
|
2: lambda: ("MaxCargoPlusHeaderWrite", struct.unpack("H", V)),
|
|
3: lambda: ("MaxCargoPlusHeaderRead", struct.unpack("H", V)),
|
|
4: lambda: ("MaxTransferWrite", struct.unpack("H", V)),
|
|
5: lambda: ("MaxTransferRead", struct.unpack("H", V)),
|
|
6: lambda: ("NormalChannel", struct.unpack("B", V)),
|
|
7: lambda: ("WakeChannel", struct.unpack("B", V)),
|
|
8: lambda: ("AppName", (V[:-1].decode(), )),
|
|
9: lambda: ("ChannelName", (V[:-1].decode(), )),
|
|
0x80: lambda: ("SHTP Version", (V[:-1].decode(), )),
|
|
}.get(T, lambda: (str(T), (V,)))
|
|
(nm,f) = decoder()
|
|
print("%5d %-26s: %r" % ((len(ad), nm, ) + f))
|
|
ad = ad[2+L:]
|
|
|
|
if __name__ == '__main__':
|
|
i2 = I2CDriver(sys.argv[1])
|
|
|
|
d = BNO080(i2)
|
|
while True:
|
|
print('quaternion:', d.read_quaternion())
|