IMU Accelerometer & Gyroscope

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