|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import cv2 |
| 4 | +import depthai as dai |
| 5 | +import time |
| 6 | +import math |
| 7 | + |
| 8 | +print("Warning! Flashing IMU firmware can potentially soft brick your device and should be done with caution.") |
| 9 | +print("Do not unplug your device while the IMU firmware is flashing.") |
| 10 | +print("Type 'y' and press enter to proceed, otherwise exits: ") |
| 11 | +if input() != 'y': |
| 12 | + print("Prompt declined, exiting...") |
| 13 | + exit(-1) |
| 14 | + |
| 15 | +# Create pipeline |
| 16 | +pipeline = dai.Pipeline() |
| 17 | + |
| 18 | +# Define sources and outputs |
| 19 | +imu = pipeline.create(dai.node.IMU) |
| 20 | +xlinkOut = pipeline.create(dai.node.XLinkOut) |
| 21 | + |
| 22 | +xlinkOut.setStreamName("imu") |
| 23 | + |
| 24 | +# enable ACCELEROMETER_RAW at 500 hz rate |
| 25 | +imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 500) |
| 26 | +# enable GYROSCOPE_RAW at 400 hz rate |
| 27 | +imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 400) |
| 28 | +# it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections |
| 29 | +# above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available |
| 30 | +imu.setBatchReportThreshold(1) |
| 31 | +# maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it |
| 32 | +# if lower or equal to batchReportThreshold then the sending is always blocking on device |
| 33 | +# useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes |
| 34 | +imu.setMaxBatchReports(10) |
| 35 | + |
| 36 | +# Link plugins IMU -> XLINK |
| 37 | +imu.out.link(xlinkOut.input) |
| 38 | + |
| 39 | +imu.enableFirmwareUpdate(True) |
| 40 | + |
| 41 | +# Pipeline is defined, now we can connect to the device |
| 42 | +with dai.Device(pipeline) as device: |
| 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 | + acceleroValues = imuPacket.acceleroMeter |
| 56 | + gyroValues = imuPacket.gyroscope |
| 57 | + |
| 58 | + acceleroTs = acceleroValues.timestamp.get() |
| 59 | + gyroTs = gyroValues.timestamp.get() |
| 60 | + if baseTs is None: |
| 61 | + baseTs = acceleroTs if acceleroTs < gyroTs else gyroTs |
| 62 | + acceleroTs = timeDeltaToMilliS(acceleroTs - baseTs) |
| 63 | + gyroTs = timeDeltaToMilliS(gyroTs - baseTs) |
| 64 | + |
| 65 | + imuF = "{:.06f}" |
| 66 | + tsF = "{:.03f}" |
| 67 | + |
| 68 | + print(f"Accelerometer timestamp: {tsF.format(acceleroTs)} ms") |
| 69 | + print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}") |
| 70 | + print(f"Gyroscope timestamp: {tsF.format(gyroTs)} ms") |
| 71 | + print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ") |
| 72 | + |
| 73 | + if cv2.waitKey(1) == ord('q'): |
| 74 | + break |
0 commit comments