From 1c4359abd0a9cd7573ce40962a054661f6153a10 Mon Sep 17 00:00:00 2001
From: James Bowman <jamesb@excamera.com>
Date: Mon, 4 Mar 2019 11:04:06 -0800
Subject: [PATCH] BNO080 IMU sample

---
 python/samples/bno080.py | 210 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 210 insertions(+)
 create mode 100644 python/samples/bno080.py

diff --git a/python/samples/bno080.py b/python/samples/bno080.py
new file mode 100644
index 0000000..6a41a03
--- /dev/null
+++ b/python/samples/bno080.py
@@ -0,0 +1,210 @@
+"""
+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())