IMU Rotation Vector

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6import math
7
8device = dai.Device()
9
10imuType = device.getConnectedIMU()
11imuFirmwareVersion = device.getIMUFirmwareVersion()
12print(f"IMU type: {imuType}, firmware version: {imuFirmwareVersion}")
13
14if imuType != "BNO086":
15    print("Rotation vector output is supported only by BNO086!")
16    exit(1)
17
18# Create pipeline
19pipeline = dai.Pipeline()
20
21# Define sources and outputs
22imu = pipeline.create(dai.node.IMU)
23xlinkOut = pipeline.create(dai.node.XLinkOut)
24
25xlinkOut.setStreamName("imu")
26
27# enable ROTATION_VECTOR at 400 hz rate
28imu.enableIMUSensor(dai.IMUSensor.ROTATION_VECTOR, 400)
29# it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
30# above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
31imu.setBatchReportThreshold(1)
32# maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
33# if lower or equal to batchReportThreshold then the sending is always blocking on device
34# useful to reduce device's CPU load  and number of lost packets, if CPU load is high on device side due to multiple nodes
35imu.setMaxBatchReports(10)
36
37# Link plugins IMU -> XLINK
38imu.out.link(xlinkOut.input)
39
40# Pipeline is defined, now we can connect to the device
41with device:
42    device.startPipeline(pipeline)
43
44    def timeDeltaToMilliS(delta) -> float:
45        return delta.total_seconds()*1000
46
47    # Output queue for imu bulk packets
48    imuQueue = device.getOutputQueue(name="imu", maxSize=50, blocking=False)
49    baseTs = None
50    while True:
51        imuData = imuQueue.get()  # blocking call, will wait until a new data has arrived
52
53        imuPackets = imuData.packets
54        for imuPacket in imuPackets:
55            rVvalues = imuPacket.rotationVector
56
57            rvTs = rVvalues.getTimestampDevice()
58            if baseTs is None:
59                baseTs = rvTs
60            rvTs = rvTs - baseTs
61
62            imuF = "{:.06f}"
63            tsF  = "{:.03f}"
64
65            print(f"Rotation vector timestamp: {tsF.format(timeDeltaToMilliS(rvTs))} ms")
66            print(f"Quaternion: i: {imuF.format(rVvalues.i)} j: {imuF.format(rVvalues.j)} "
67                f"k: {imuF.format(rVvalues.k)} real: {imuF.format(rVvalues.real)}")
68            print(f"Accuracy (rad): {imuF.format(rVvalues.rotationVectorAccuracy)}")
69
70
71        if cv2.waitKey(1) == ord('q'):
72            break